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This  study  describes  the  development  of  the  theory  for  articulation  of  spatial 
robots  with  special  geomctiy  such  as  the  PUMA  and  TRS  robots.  The  theory  for 
articulation  consists  of  the  analysis  of  geometrical  capability  of  a robot  and  the 

planning  problem  is  demonstrated  for  the  PUMA  robot.  The  geometric  capability 
of  a robot  is  analyzed  by  use  of  robot  attributes  such  as  workspace  and  dexterity. 
The  investigation  of  permissible  orientations  for  the  regional  structure  of  a robot 

The  geometry  of  interference  was  developed  by  investigating  the 
representation  of  an  object  placed  in  Euclidean  3-space.  By  using  affine  coordinates, 
a representation  scheme  of  simplexes  provided  a simple,  yet  powerful, 


computational  method  for  checking  the  intersection  of  objects.  A general 
displacement  of  an  object  with  respect  to  a reference  object  was  investigated  so  as 

mapping  was  also  derived  and  used  as  a rapid  decision-making  tool  for  collision- 
free  path  planning. 

The  novel  idea  of  "Machine  Intelligence  of  Robots  via  Geomcuy"  was 

articulation  takes  on  the  role  of  a basis  for  preprocessing  a robot  working 

Finally,  an  automatic  planning  software  which  demonstrates  the  application 
capability  of  the  theory  for  articulation  of  a spatial  robot  was  developed  by  fully 


CHAPTER  1 
INTRODUCTION 


automatic  robot  task  planning  systems.  It  is  also  important  in  the  sense  that  robots 
should  be  operated  safely  without  interfering  with  each  other  if  any  cooperative 
function  of  multiple  robots  is  required.  Although  a major  advantage  of  robots  over 
typical  mechanisms  is  their  variety  of  applications,  the  applications  have  been 
restricted  within  only  narrow  limits  mainly  because  of  the  difficulty  in  programming 
the  robots  — in  other  words,  the  lack  of  capabilities  of  automatically  dealing  with 
required  tasks.  It  is,  of  course,  not  because  open  chain  type  mechanisms  are  not 
suitable  for  industrial  or  some  other  complex  applications,  but  rather  because  they 
are  not  equipped  with  certain  degrees  of  machine  intelligence.  Research  on  the 
development  of  such  machine  intelligence  is  often  characterized  by  the  title  of  robot 
task  planning. 

Robot  task  planning  involves  1)  world  modeling,  2)  task  specification,  and 
3)  planning  phases  [1],  The  world  model  may  be  obtained  by  making  use  of  a 
computer-aided-design  database.  A computer  vision  system  will  eventually  provide 
the  robot  task  planner  with  the  necessary  information  of  the  robot  working 
environment  Research  into  obstacle  avoidance  is  involved  in  the  second  and  third 


phases  of  task  planning,  mainly  in  the  planning  phase.  Therefore,  the  underlying 
assumption  in  obstacle  avoidance  is  that  the  world  model  is  set  up  and  the  geometric 
information  of  the  working  environment  is  available.  The  ultimate  objective  of  the 
research  is  to  minimize  any  human  involvement  in  those  phases  once  the  working 
environment  is  known,  no  matter  how  many  obstacles  there  may  be.  It  is  important 

The  general  statement  of  obstacle  avoidance  for  a manipulator  may  be  given  as 
follows: 

For  a given  manipulator  with  the  configurations  at  the 
starting  and  the  goal  positions  placed  in  a known  working 
environment,  find  a continuous  path  for  the  reference  point 

which  does  not  cause  any  part  of  the  manipulator  to  collide 


In  fact,  a great  deal  of  effort  toward  the  goal  has  been  made  by  a number  of 
researchers,  especially  for  the  theoretical  algorithmic  development  of  motion 
planning  [2,  3],  There  has  been  some  remarkable  progress  in  the  research  of 
obstacle  avoidance,  for  example,  1)  the  problem  of  intersection  of  solids,  2) 
dynamic  collision  detection,  3)  the  algorithmic  approach  and  the  study  of  the  time 
complexity  of  planning  algorithms,  and  so  on.  In  spite  of  such  research  progress 
over  the  last  ten  years,  it  is  not  likely  that  the  development  of  a general  analytical 
theory  can  be  easily  accomplished,  simply  because  of  the  inherently  complicated 
nature  of  the  problem. 

In  the  problem  of  obstacle  avoidance,  all  the  necessary  geometric 
descriptions  of  the  robot  working  environment  are  assumed  to  be  provided.  One 
manipulator  might  be  a six  degree-of-freedom  serial  manipulator  with  revolute  joints 


rithout  any  physical  turning  limits  of  i 


s.  It  might  be  impc 


to  observe  some  characteristics  of  today's  industrial  manipulators,  and  also,  to 
introduce  some  terminologies  frequendy  used  in  this  field  of  research.  For  six 
degree-of-frecdom  serial  manipulators,  the  part  of  the  manipulator  consisting  of  the 

characteristics  of  many  industrial  robots  is  that  their  regional  structures  have  some 
special  geometrical  configurations.  If  only  the  robots  consisting  of  revolute  joints  arc 
considered,  then  one  such  special  configuration  is  that  the  first  two  joint  axes 
intersect  at  a certain  angle  (usually  perpendicular)  and  the  second  and  the  third 
joint  axes  are  parallel  (for  example,  Coiffet  [4]  shows  that  25%  of  today’s  robots 
have  this  type  of  configuration). 

For  six  degree-of-freedom  manipulators,  the  part  consisting  of  the  last  three 

geometrical  characteristic  of  many  industrial  robots  is  that  the  last  three  joint  axes 
intersea  at  a point  which  is  called  a wrist  center.  In  this  research,  the  theoretical 
development  of  obstacle  avoidance  will  be  restriaed  to  six  degree-of-freedom 
manipulators  with  such  special  configurations  on  the  regional  structure  and  a clearly 
defined  wrist  center. 

The  manipulator  motions  may  be  divided  into  two  general  categories,  a fins 
motion  such  as  grasping  and  insertion  and  a gross  motion  such  as  the  transfer 
movement  of  the  manipulator  arm.  Observing  that  much  of  the  gross  motion  of  the 
manipulator  depends  on  the  movement  of  the  regional  structure  and  that  the 
physical  linkages  (usually  the  upper  arm  and  the  forearm)  of  this  structure  are  large 
enough  to  easily  collide  with  obstacles,  it  seems  important  to  consider  this  structure 


for  the  gross  motion  planning.  The  consideration  of  only  four  degrees  of  freedom 
may  be  sufficient  for  pick-and-place  operations  [5].  While  the  payload  is  being 
carried  by  the  manipulator  from  one  position  to  another,  the  only  necessary 
orientational  motion  may  be  a rotation  about  the  vertical  axis  [5,  6).  In  fact, 
collision-free  path  planning  for  the  regional  and  the  orientational  structures  can  not 
be  performed  separately.  However,  it  is  convenient  to  conceptually  split  the  topic 

Before  reviewing  the  previous  works  on  this  topic,  the  following  definitions 
will  be  given  for  their  frequent  appearance  in  the  literature: 

Find-space  problem:  For  a given  moving  object,  find 

some  specified  region  in  which  the 
object  can  be  placed  without 
colliding  with  any  other  objects. 

Find-nath  problem:  For  a given  object  with  specified 

configurations  at  the  starting  and 
the  goal  positions,  find  a path 
so  that  the  object  does  not  collide 
with  any  other  objects  while  it  is 
moving  between  the  two  positions  [7], 

As  mentioned  earlier,  there  have  been  many  fields  of  research  related  to 
obstacle  avoidance,  for  example,  the  intersection  problems  of  solids  [8],  dynamic 
collision  detection  [9],  the  algorithmic  approach  and  the  study  of  the  time 
complexity  of  planning  algorithms  [2,  3].  A brief  historical  review  of  obstacle 

that  done  by  Udupa  [6]  can  be  regarded  as  the  first  work  on  obstacle  avoidance  for 
a computer  controlled  spatial  manipulator,  and  this  work  introduced  the  free  space 
(whose  definition  will  be  given  later  in  detail)  method.  Udupa  developed  the  basic 
idea  of  the  'growing  transformation"  (in  (act,  this  terminology  was  introduced  later) 


approach  for  the 


connected  bounding  cylinders,  one  each  for  the  post,  boom,  and  forearm,  and  the 
obstacles  are  approximated  by  polyhedra.  The  polyhedra  are  then  enlarged  by  the 
growing  transformation  so  that  the  boom  can  be  modeled  as  a line-segment  and, 
finally  as  a point.  The  algorithm  finds  a safe  path  for  a reference  point  (the  boom 
tip)  by  recursively  introducing  intermediate  points  into  a straight  line  path  in  free 
space.  This  path  is  then  heuristically  modified  in  order  to  reorient  the  forearm. 
This  concept  of  the  growing  transformation  was  further  developed  by  Lozano-Perez 
and  Wesley  (10].  The  method  of  performing  the  exact  two  dimensional  growing 
transformation  was  described  by  making  use  of  the  concept  of  "mixing"  (see 
Lyustemik  [11])  from  the  theory  of  convex  sets  and  the  generalization  to  the  three 
dimensional  case  was  discussed.  The  work  also  introduced  the  technique  of 
VGRAPH  (called  visibility  graph),  searching  a safe  path-segment  connecting  the 
vertices  of  the  grown  obstacles  (polygons)  that  can  "see"  each  other.  This  technique 
was  extended  in  its  applications  to  the  three  dimensional  problem  [12].  The 
algorithm  begins  by  constructing  grown  obstacles.  The  space  outside  the  obstacles 
is  decomposed  into  many  hybrid  cells  (rectangular  and  arbitrary  convex  polyhedra), 
so  as  to  avoid  huge  numbers  of  cells  of  uniform  shape  or  size.  Finally,  it  searches 
a connected  set  of  such  cells  using  a heuristic  search  algorithm.  This  technique  was 
successfully  applied  only  to  Cartesian  type  robots  because  of  a difficulty  with  the 
rotational  growing  transformation.  The  concept  of  the  growing  transformation  seems 
important  and  useful,  though  the  major  difficulty  with  it  is  that  it  has  to  be 
performed  each  time  the  moving  object  is  allowed  to  rotate.  Therefore,  if  the 
moving  object  changes  its  orientation  continuously,  then  it  is  practically  impossible 


lo  continuously  compute  the  transformed  obstacles.  Furthermore,  if  the  moving 
object  is  a many  degree-of-freedom  manipulator,  it  accelerates  the  complexity  of  the 
problem. 

Lozano-Perez  (7)  developed  the  mathematical  treatment  of  the  configuration 
space.  The  term  configuration  space  (denoted  here  by  CS)  is  defined  to  represent 
all  the  possible  sets  of  manipulator  joint  variables.  The  space  containing  all  the 
possible  sets  of  joint  variables  with  respect  to  a reference  point  of  a manipulator  that 
cause  any  collision  with  the  obstacles  is  referred  to  as  the  configuration  space 
obstacle  (denoted  here  by  CSO).  The  complement  of  the  CSO  with  respect  to  the 
CS  is  the  space  in  which  the  manipulator  can  move  without  collision;  this  space  is 
called  the  free  space  (FS).  An  algorithm  was  developed  in  such  a way  that  it  can 
shrink  a moving  object  down  to  a point  (a  reference  point)  by  transforming  the 

becomes  the  problem  of  representation  of  the  FS.  Once  the  FS  is  known,  then  the 
algorithm  searches  for  a path  in  this  FS.  Performing  the  growing  transformation  of 

Even  when  a serial  manipulator  modeled  by  the  connected  polyhedra  is  dealt  with, 
it  is  impossible  (in  a practical  sense)  to  represent  the  grown  obstacles  on  the 
computer.  This  observation  helps  explain  the  complexity  of  the  find-path  problem 
for  a manipulator.  It  is  obvious  that  the  CS,  CSO,  and  FS  are  all  six  dimensional 
spaces  for  a typical  six  degree-of-freedom  industrial  robot.  Note  especially  that  the 
CSO  is  a complicated  six  dimensional  object  in  general.  In  fact,  the  rotational 
movement  of  the  object  causes  this  difficulty  with  explicit  representation  of  the  CSO. 
For  this  reason,  the  application  of  the  algorithm  was  restricted  to  translational 
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The  configuration 


approach  was  further  developed  to  overcome  the  difficulties  with  rotations  [13]  by 
use  of  the  recursive  cellular  representation  for  the  CS  for  two  dimensional  polygonal 
objects.  The  major  drawback  of  this  approach  lies  in  the  fact  that  it  is  not 
practically  possible  to  apply  the  concept  of  the  growing  transformation  to  spatial 
manipulators  having  more  than  three  degrees  of  freedom. 

Brooks  presented  an  algorithm  for  solving  the  find-path  problem  by 
introducing  the  idea  of  "freeways"  represented  as  generalized  cones  for  the  two 
dimensional  avoidance  problem  [14],  In  this  technique,  a generalized  cone  is 
formed  by  sweeping  a two  dimensional  polygon  along  a straight  line,  called  a spine. 
It  examines  the  outward  normals  of  all  pairs  of  edges  of  obstacle  polygons  for  a 
candidate  generalized  cone  so  that  it  can  construct  a spine  as  a bisector.  When  the 
generalized  cones  arc  constructed,  they  are  examined  in  pairs  to  determine  whether 
the  spines  intersect  and  where.  The  feasibility  of  the  movement  through  any 
candidate  freeway  is  tested  by  comparing  the  width  of  the  bounding  rectangle  of  a 
moving  object  with  that  of  the  freeway.  The  width  of  the  bounding  rectangle  can  be 
changed  and  determined  by  the  radius  function  corresponding  to  the  current 
orientation  of  the  moving  object.  As  a final  stage,  it  searches  a path  from  a starting 
to  a goal  position  by  changing  from  one  cone  to  another  at  the  intersections  of  the 
spines.  The  orientation  of  the  moving  object  remains  unchanged  for  one  selected 
spine  and,  if  necessary,  changes  its  orientation  at  the  intersection  of  the  spines. 
The  application  of  this  method  was  extended  later  to  motion  planning  for  pick-and- 
place  operations  [5].  In  this  work,  obstacles  are  modeled  by  prisms  supported  from 
below  and  hanging  from  above.  The  cross  sections  of  the  obstacles  are  convex.  A 


pair  of  overhead  cameras  were  used  to  provide  the  world  information.  By  fixing 
joint  4 and  rotating  joint  6 only  about  the  vertical  axis,  paths  are  found  where  the 
payload  is  moved  in  straight  lines,  either  horizontal  or  vertical.  Separate  freeways 
are  considered  for  the  upper  arm  and  for  the  hand  and  payload  ensemble.  The  two 
freeway  spaces  are  searched  concurrently  under  projection  of  constraints  determined 
by  the  forearm  motion.  It  Gist  finds  freeways  for  the  upper  arm.  Then  it  searches 
the  space  of  payload  freeways  in  which  the  determined  path  of  the  upper  arm  is  safe. 
The  algorithm  was  implemented  particularly  for  the  PUMA  robot.  Nevertheless, 

been,  in  fact,  introduced  by  Udupa  [6].  The  concept  of  decomposition  was  not 
mathematically  well  developed.  The  freeway  concept  still  requires  an  efficient 
search  algorithm. 

Some  works  taking  different  approaches  other  than  the  configuration  space 
approach  can  be  also  found  [15-19].  One  common  fact  that  can  be  found  in  a 
number  of  researches  is  that  a search  algorithm  (usually  heuristic)  is  used  for  the 
find-path  problem.  An  algorithm  generating  CSO’s  for  obstacles  given  by  segments 
of  planar  algebraic  curves  can  be  found  in  [20].  Lumelsky  [21-23]  took  geometrical 
approaches  to  the  planning  problem  for  planar  robots  amongst  unknown  obstacles 


robot  [24], 


1,2  A Theory  fo 


Although  there  have  been  many  research  results  as  described  in  the  previous 

section,  many  investigators  have  taken  the  configuration  space  approach  to  the 

one  of  the  major  goals  in  the  research  may  be  the  representation  scheme  of 
configuration  space  obstacles  in  higher  dimensional  space.  When  configuration  space 
obstacles  are  represented  on  a computer,  a heuristic  search  technique  is  employed 
for  path  planning.  This  implies  that  the  computational  efficiency  of  algorithms  may 
be  heavily  dependent  on  the  computing  power  instead  of  on  the  algorithms.  Such 
a high  dependency  on  the  computing  power  may  be  reduced  by  taking  a different 
approach  to  the  problem,  i.e.,  a theory  for  robot  articulation. 

It  has  been  demonstrated  that  robot  geometries  such  as  workspace  and 
dexterity  can  be  best  used  in  path  planning  [25-31].  It  was  shown  [28]  that  various 
robot  motions  are  possible  in  the  presence  of  obstacles  via  robot  geometry  without 
computing  successive  sets  of  joint  angles.  The  first  research  results  of  the  theory  of 
articulation  were  presented  by  Young  and  Duffy  [29,  30].  Based  on  the  research 
results  from  Lipkin  et  al.  [25, 28],  a theory  for  robot  articulation  for  a planar  robot 
was  developed  by  analyzing  flexure  of  a 3R  planar  robot  and  formulating  the 
mapping  of  line  interference. 

In  this  research,  extended  from  the  kinematic  analysis  of  a 3R  planar  robot, 
analysis  of  the  geometric  capability  of  TRS  and  PUMA  type  robots  was  carried  out. 
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The  mapping  of  line  interference  was  further  investigated  by  taking  a different 
approach  from  that  presented  by  Young  and  Duffy  [29, 30]  in  order  to  develop  the 
geometiy  of  interference  for  a general  three-dimensional  problem.  Analysis  of  the 
geometric  capability  of  a robot  provides  information  about  the  permissible 
orientations  of  the  links  of  a robot,  while  the  geometiy  of  interference  contributes 

concept  of  articulation  theory  is  to  make  use  of  the  geometrical  capability  of  a robot 

with  the  links  of  robots. 

In  contrast  to  other  planning  approaches,  the  geometty-based  algorithms 
(utilizing  geometric  properties  such  as  workspace  and  dexterity)  guarantee  significant 
reduction  of  computation  time  in  path  planning.  This  has  been  one  of  the  most 
important  issues  in  this  research  area 


The  goal  of  this  research  is  to  develop  an  articulation  theory  for  spatial  robots 
and  the  application  techniques  for  collision-free  path  planning  problems.  This  theory 

ingredients.  In  the  following  chapter,  analysis  of  the  geometric  capability  of  a 
robot  will  be  described.  This  analysis  will  involve  both  the  workspace  and  dexterity. 
Analysis  of  the  robot's  geometric  capability  is  important  in  developing  the  theory  of 
articulation  since  the  information  provides  the 
a robot  subject  to  obstacle  avoidance. 


capability  of  i 


In  chapter  3,  development  of  the  geometty  of  interference  will  be  described 
in  detail.  This  provides  a purely  geometrical,  yet  powerful,  tool  for  interference 
checking  of  two  objects.  The  geometty  of  interference  is  developed  by  interpreting 
a "Euclidean'  as  a special  case  of  an  affine  transformation  in  Euclidean  3-space.  In 

will  later  be  shown  how  such  an  intersection  can  be  utilized  in  the  path  planning 
process.  It  will  be  also  shown  that  this  development  of  interference  geometry  is  the 
full  extension  of  the  previous  work  presented  in  [29,  30],  that  is,  the  two 
dimensional  problem  is  extended  to  the  three  dimensional  case.  In  chapter  4,  it  will 
be  demonstrated  how  the  result  of  this  development  can  be  applied  to  the  collision- 
free  path  planning. 

Chapter  4 is  an  implementation  of  the  novel  idea  of  "Machine  Intelligence 
of  Robots  via  Geometry"  for  path  planning,  and  an  integration  of  applications  with 
the  theory.  When  the  result  of  this  development  is  implemented  by  means  of 
computer  algorithms  for  planning,  it  takes  on  the  role  of  a basis  for  preprocessing 
a robot  working  environment  which,  in  turn,  provides  the  planning  system  with 
decision-making  capability i.e.,  machine  intelligence. 

Finally,  application  of  the  articulation  theory  will  be  demonstrated  by  the 
implementation  of  automatic  planning  software,  fully  utilizing  computer  graphics 
animation  with  realistic  visualization  of  variable  simulations.  The  PUMA  robot  has 
been  selected  in  order  to  demonstrate  application  of  the  theory  to  obstacle 


CHAPTER  2 

THE  GEOMETRIC  CAPABILITY  OF  A ROBOT 


2.1  Background 

In  the  elementary  kinematics  of  robots,  it  is  well  known  that  for  a given 
position  and  orientation  of  the  end  effector  of  a general  6R  robot,  there  exists  a 
futile  number  of  possible  configurations.  However,  when  only  the  position  (not  the 
orientation)  of  the  end  effector  is  specified,  an  infinite  number  of  possible 
configuratioas  exist.  In  other  words,  each  of  the  robot  links  can  be  rotated  around 
the  joint  axes  by  some  angles  while  the  end  effector  is  placed  at  a specified  position. 

let  us  first  consider  a planar  3R  robot  having  three  degrees  of  freedom  or  the  overall 
mobility,  M = 3.  Therefore,  three  parameters  are  needed  in  order  to  completely 
specify  the  position  and  orientation  of  the  robot.  However,  when  the  position  (but 
not  orientation)  of  the  end  effector  is  given,  the  mechanism  has  mobility,  M = 1. 
When  the  position  (but  not  orientation)  of  the  end  effector  of  a spatial  6R  robot  is 
specified,  the  robot  still  has  three  degrees  of  freedom  or  mobility,  M = 3.  This 
means  that  three  more  parameters  have  to  be  specified  so  as  to  completely  define 
a configuration  of  the  robot.  This  observation  leads  to  the  essential  idea  of  robot 
articulation.  The  freedom  in  choosing  values  of  these  parameters,  while  the 
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position  of  end  effector  is 


ailing  the 
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to  avoid  obstacles  and  interference  with  other  robots. 

When  a general  6R  robot  is  given  with  its  end  effector  at  a specific  position, 
the  reference  point  of  the  end  effector,  denoted  by  P„  may  be  replaced  by  a 
spherical  joint  in  order  to  analyze  the  geometrical  capability.  Thus,  we  have  6RS 
spatial  mechanism.  The  analysis  begins  by  dividing  this  6RS  mechanism  into  3R  and 
S2R  spatial  robots  at  the  wrist  center  as  shown  in  Figure  2.1.  The  analysis  then 
proceeds  to  the  workspace  considerations  of  the  wrist  center  of  the  3R  and  the  S2R 
robots  respectively. 

Analysis  of  robot  workspaces  has  been  broadly  carried  out  by  many 
researchers  because  of  its  importance  in  designing  a robot.  It  seems  that  most  of  the 
research  has  been  centered  especially  around  robots  with  turning  joints.  Amongst 
those  presented  research  results,  two  papers  may  be  considered  as  the  most 

arm  is  a torus  and  that  many  different  shapes  of  tori  can  be  generated  by  changing 
the  mechanism  dimensions.  Sugimoto  and  Daffy  [3334]  proved  two  important 
theorems  on  the  extreme  distances  of  a robot  hand  and  showed  that  there  are  eight 
possible  extreme  reaches  for  a 3R  robot  arm  with  arbitrary  dimensions.  Many  other 
research  results  along  this  line  can  be  found  in  additional  references  [35  - 46). 

Although  the  workspace  analysis  of  general  3R  robots  can  be  carried  out  by 
employing  some  mathematics  [42  - 46],  some  special  geometries  such  as  o,2  = */2 
and  oa  = 0 (refer  to  Figure  23  for  notation)  are  often  adopted  for  the  design  of 
3R  robots 


i (or  regional  structures  of  6R  robots) : 


Orientational 


Figure  2.1  Regional  and  Orientational  Structures 


[41].  In  this  research,  the  PUMA  and  TRS  type  robots  with  such  special  geometries 
within  their  regional  structures  will  be  considered.  Due  to  such  special  geometries, 
analytical  expressions  for  the  workspace  boundary  surfaces  can  be  easily  obtained. 
When  the  workspace  of  the  S2R  robot  represnting  an  orientational  structure  is 
found,  the  geometric  capability  of  a hand  is  then  determined  by  the  intersection  of 
the  workspaces  of  the  wrist  center  with  respect  to  the  origin  of  the  fixed  frame  and 
a specified  position  of  the  reference  point  of  the  end  effector. 


The  Elbow  Point  and  the  Wrist  Center.  The  elbow  point  of  a robot  is  the 
center  point  of  the  elbow  joint  and  denoted  by  Pt„,.  The  wrist  center  of  a robot  is 
the  point  where  three  turning  joints  of  an  orientational  structure  intersect  and  will 
be  denoted  by  P„  For  an  orientational  structure  whose  three  turning  axes  do  not 
intersect  at  a point,  the  point  P„  will  be  placed  at  the  center  of  the  first  joint  of  the 

of  the  turning  joint  of  the  link  to  which  an  end  effector  is  attached. 

Hand.  A hypothetical  link  joining  the  wrist  center  to  the  reference  point, 
for  example  the  center  of  an  end  effector,  is  referred  to  as  the  hand  of  a robot 
When  the  mechanism  dimensions  of  a robot  are  known,  the  length  of  the  hand  can 
be  immediately  determined  and  will  be  denoted  by  a**, 


rkspace  of  an  elbow  point  is  denoted  by 


Figure  2.2  Terminology  and  Notation 


TRS  and  PUMA  type  robots,  respectively.  The  workspace  of  a 

The  workspace  of  the  wrist  center  with  respect  to  the  reference  pc 
effector  is  referred  to  here  as  a sphere  of  hand  orientation  and  will 


Profile  Plane.  Consider  now  a center-line  of  the  upper  arm  of  a manipulator. 

sweeps  a plane.  This  plane  will  be  referred  to  as  the  profile  plane  with  regard  to 
the  center-line.  For  example,  let  us  consider  the  PUMA  robot.  It  has  a cylindrical 
hole  in  W,  A vertical  plane  tangent  to  this  cylindrical  hole  is  then  a profile  plane 
of  the  center-line  of  the  upper  arm.  For  convenience  in  the  following  development, 
the  origin  of  the  fixed  reference  frame  will  be  placed  at  the  intersection  of  the  first 
two  turning  joints. 

Orientational  and  Geometric  Capability.  The  term  orientational  capability 
of  a robot  hand  is  used  to  represent  the  angular  range  of  the  wrist  about  a specified 
position  of  the  reference  point  of  the  end  effector  (not  about  the  wrist  point)  within 
the  robot’s  workspace.  When  the  angular  range  becomes  360  degrees,  it  is  said  that 
the  robot  has  full  capability  of  orientation  at  a specified  position  of  the  end  effector, 

represent  the  permissible  angular  ranges  of  each  of  the  turning  joints.  Therefore, 
the  representation  of  geometric  capability  of  a robot  includes  that  of  onemauonal 


capability. 


2.2.2  Goals 


The  major  goal  in  this  chapter  is  to  investigate  the  geometric  capability  of 
TRS  and  PUMA  robots  under  certain  constraints  so  that  the  knowledge  of  geometric 
capability  of  a robot  can  be  utilized  for  interference  avoidance  of  robot  links  and/or 
obstacle  avoidance.  Basically,  the  geometric  capability  of  such  robots  can  be 
measured  as  the  angular  range  of  each  of  the  joints  for  the  corresponding  6RS 
mechanism.  This  can  be  obtained  by  connecting  a hand  to  ground  via  a spherical 
joint  which  replaces  the  end  effector.  In  order  to  completely  understand  a robot's 
geometric  capability,  analysis  begins  with  the  workspace  of  the  wrist  center.  Finally, 
the  rotational  capability  of  each  of  the  joints  of  the  regional  structure  will  be 
investigated.  For  its  later  application,  kinematic  analysis  of  the  geometric  capability 
of  those  robots  with  infinitely  long  hands  will  be  also  investigated. 


2.3  Orientational  Capability  of  a Robot 


Let  us  Erst  consider  a general  3R  robot  arm  (see  Figure  2.3)  and  suppose  that 
all  the  turning  joints  are  ideal  (no  angular  limits).  Using  the  notation  in  Figure  2.3 
(see  also  Duffy  [47]),  the  displacement  equation  of  point  A in  Figure  23  is 
R*  - [X,  Y,  Zf 
3 

= MS.S,  + a,  (2-1) 


S,  = (0.0,1) 


(CpS.,0) 
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\A 


Figure  23  Nolalion  for  a 3R  Robot 


h = (Slft.-SllCi.Cu) 

Sj  = (x„,x,;,  z,) 


(2-2) 


2a  = (WnrVa.Ua) 

a*  = (WjjL-Vm.Ujj,) 

Uji  = Vn  v2i  = -(s*  + W,,)  Wa  = (cjCj  - s.SjCu) 

Xj,  = XjC,  - YjS,  X,  = S23S2 

X2;  = XjS,  + YjC,  Yj  = -(s.jCj,  + c^jjCj) 

^2  = (^U®23  " 

Un,  = UjjCn  - VjjSu  Uj,  = SjSa 

VHi  = Ci(U»Si2  + Vnc,2)  - SlWE  Vn  = -(SjCj  + CjSjCjj) 

Wni  = s,(UjjSi2  + VjjCu)  + c,Wjj  W„  - (c*  - SjSjCjj)  (2-3) 

Obviously,  the  workspace  of  the  point  P.  of  the  end  effector  is  the  set  of  all  the 
points  satisfying  the  above  equation-  The  expressions  for  the  workspace  boundary 
surfaces  are  derived  as  follows: 

TRS  Robots  The  special  geometry  of  TRS  robots  is  as  follows  (see  Figure 

2.4): 

au  = 0 a, 2 = x/2  or  3x/2 

So  = 0 S,  //  S3  (2-4) 

Placing  the  origin  of  the  fixed  frame  at  the  intersection  of  the  axes  of  first  two 
turning  joints,  we  get  the  following  displacement  equation: 

(2-5) 


ajjaa  + 03,834 


Figure  2.4  a TRS  Robot  and  the  Workspace 
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2.2.2  finals 

The  major  goal  in  this  chapter  is  to  investigate  the  geometric  capability  of 
TRS  and  PUMA  robots  under  certain  constraints  so  that  the  knowledge  of  geometric 
capability  of  a robot  can  be  utilized  for  interference  avoidance  of  robot  links  and/or 
obstacle  avoidance.  Basically,  the  geometric  capability  of  such  robots  can  be 
measured  as  the  angular  range  of  each  of  the  joints  for  the  corresponding  6RS 
mechanism.  This  can  be  obtained  by  connecting  a hand  to  ground  via  a spherical 
joint  which  replaces  the  end  effector.  In  order  to  completely  understand  a robot's 
geometric  capability,  analysis  begins  with  the  workspace  of  the  wrist  center.  Finally, 
the  rotational  capability  of  each  of  the  joints  of  the  regional  structure  will  be 
investigated.  For  its  later  application,  kinematic  analysis  of  the  geometric  capability 
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X = ag^Cj  + aMc,C2,j  (2-6a) 

Y = aas,C2  + aj,s,C2,j  (2-6b) 

Z = ajjSj  + a^Sj.j  (2-6c) 

Eliminating  sines  and  cosines  (for  angles  1 and  2)  from  the  above  two  displacement 

XI  + Y2  + Z2  = aj  + a,?  + 202385*  (2-7) 

Since  C3  must  lie  between  -1  and  +1,  we  have 

X2  + Y1  + Z1  i (823  - a»)J  (2-8a) 

X2  + Y2  + Z2  s (ajj  + a,,)2  (2-8b) 


These  represent  the  inner  and  outer  boundary  surfaces,  i.e.  two  concentric  spheres. 
PUMA  Robot  The  special  geometry  of  this  robot  is  as  follows  (see  Figure 

IS)-. 

a12  = 0.  «u  = 3x/2, 

h II  §3. 

and  or,,  = rr/2.  (2-9) 

Again,  placing  the  origin  of  the  fixed  frame  at  the  intersection  of  the  axes  of  first 
two  turning  joints,  we  get  the  displacement  equation: 

R3,  = SjjSj  + a„a„  + aj^j,  + SUS,  (2-10) 

and  substituting  the  direction  cosines  yields 

X = -Sjjs,  + ajjC*  + a^c*,,  + S„c,S2,3 
Y - S22C1  + ajjS.Cj  + a3,(c,S2,3  + s 
Z = -HjjS,  - aj,S2,3  + S„C2,j 


slc2*a)  + S„s,s 


(2-1 la) 
(2-1 lb) 
(2-1 lc) 


rkspace  boundary. 


By  selling  4,  = 0.  we  can  get  the  cross-section  of  the  wor 
possible  since  the  workspace  is  symmetric  about  the  first  turning  joint  axis. 

X = aac,  + ajjCj.j  + S„Sj.,  (2- 12a) 

Y = Sja  + aj.Sj.,  (2-12b) 

Z = -a;;S.  - a J4S2 . j + SMC;.3  (2-12c) 

From  Eq.  (2-12b),  we  get 

S*  - a„  s Y s S*  + a*  (2-13) 

Squaring  and  adding  Eq.  (2-12a)  and  Eq.  (2-12c)  yields 

X1  + Z!  = aj  + ajJ  + Sai  + 2a,jaj,Cj  + 2aj3S„s3  (2-14) 

Letting  tan(«j/2)  = x and  examining  the  real  values  for  x gives  the  following: 

X2  + ZJ  k aj  + OjJ  + S«  - 2aaj(Sj  - ajJ)*  (2-lSa) 

X2  + Z2  s a^  + aji  + S„  - 2aaj(Sj  + aM2)*  (2-15b) 

This  shows  that  the  boundary  curves  on  the  X-Z  plane  are  two  concentric  circles. 
Therefore,  the  inner  boundary  surface  could  be  either  a cylindrical  or  a spherical 
surface,  or  the  combination  of  two  different  surfaces  depending  on  the  mechanism 
dimensions,  whereas  the  outer  boundary  surface  is  a sphere.  The  PUMA  robot  is 

2.3.2  The  Sphere  of  Hand  Orientation.  s„ 

In  the  beginning,  it  may  be  useful  to  consider  the  various  types  of 
orientational  structures.  Figure  2.6  illustrates  several  types  of  orientational  structures 
of  industrial  robots.  Let  us  first  consider  an  orientational  structure  with  a wrist 
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Figure  2.6  Some  One 


}f  Industrial  Robots 


reference  point  of  end  effector,  s„  is  a sphere  with  the  radius  If  the  three 

point  of  the  end  effector  so  as  to  obtain  a S2R  robot  for  which  the  S-joint  is 
attached  to  ground.  In  this  case,  the  entire  robot  can  rotate  about  the  point  Pt, 
and  the  workspace  s.  is  a spherical  shell  bounded  by  two  concentric  spheres.  The 
radii  of  the  spherical  shell  are  the  maximum  and  minimum  of  distances  from  the 
point  Pc  to  the  wrist  center  (see  Figure  2.7). 

2,3.3  The  Representation  of  Orientational  Capability 

Full  Capability  of  Orientation  As  introduced  earlier  in  this  chapter,  the 
"orientational  capability"  (similar  in  some  aspects  to  the  concept  of  "dexterity")  of  a 
robot  with  an  orientational  structure  represents  the  angular  range  of  permissible 
orientation  of  the  hand  with  respect  to  the  given  position  of  the  reference  point  of 
the  end  effector.  When  a hypothetical  spherical  joint  is  placed  to  connect  the 
reference  point  to  ground,  the  workspace  of  the  wrist  center  with  respect  to  the 
reference  point  is  a sphere  (sj.  Since  the  boundary  surface  of  the  workspace,  W„ 

robot  corresponds  to  the  common  workspace  of  Ww  and  s»-  If  the  common 
workspace  is  the  sphere  s»  itself,  it  is  said  that  the  robot  has  full  capability  of 
orientation  at  the  specific  position  of  the  end  effector.  Otherwise,  the  shape  of  the 
common  workspace  is  determined  by  that  of  W„  and  the  position  of  the  end  effector 
of  any  particular  robot  under  consideration.  For  example,  when  a 


TRS 


Figure  2.7  Sphere  of  Hand  Orientation 
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considered,  Ihe  common  workspace  corresponding  10  its  orientatioanl  capability  is 
the  spherical  surface  within  the  small  circle  on  s»  which  is  made  by  an  intersection 
Ww  and  s..  This  observation  suggests  that  the  orientational  capability  of  a robot  may 
be  best  represented  by  a spherical  region  on  s».  Figure  2,8  illustrates  the 
orientational  capability  of  a TRS-type  robot. 


permissible  range  of  the  hand  of  TRS  and  PUMA  robots  is  now  described.  Let  Pe 
denote  the  position  vector  to  the  reference  point  of  end  effector  Pt  in  the  fixed 
frame.  For  given  P,,  the  permissible  ranges  of  the  hand  as  well  as  d,  can  be 
immediately  determined  by  the  intersection  of  the  spheres  S„  and  s»-  The 
intersection  of  two  spheres  is,  in  general,  a circle  on  a plane.  This  circle  will  be 
denoted  by  Q,  The  plane  is  called  a radical  plane,  denoted  by  IIr  and  the  line 
joining  the  centers  of  the  two  spheres  is  normal  to  the  radical  plane.  The  feasible 
positions  of  the  wrist  center  are,  therefore,  those  in  the  spherical  region  on  s. 
enclosed  by  C».  Since  the  circle  C,  divides  the  entire  spherical  surface  into  two 
regions,  it  is  then  necessary  to  specify  the  region  for  feasible  positions.  The  circle 
C»  and  the  point  Pt  may  be  used  to  construct  a cone,  referred  to  here  as  a director 


orientation.  This  terminology  may  be  further  explained  from  the  elementaty  three 
dimensional  geometry  as  follows:  when  the  cone  is  constructed  as  described  above, 
the  base  circle  C„  is  the  directrix  of  the  cone.  It  is  always  possible  to  generate  an 
arbitrary  shaped  cone  with  its  vertex  at  the  reference  point  of  the  end  effector.  For 


The 


(see  Figure  2.9),  or  more  compactly,  a 


of 


nple,  if  the  workspace  of  the 


’ is  a cylinder,  the  directrix  of  the  i 


a)  full  capability  of  orientation 


b)  partial  capability  of  orientation 


Figure  2.8  Orientational  Capability  of  the  Hand 
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Figure  2.9  Cone  of  Hand  Orientalion 


will  be  a space  quanic.  However,  in  this  case,  the  analytic  expression  of  such  a 
cone  may  be  so  complicated  that  it  may  not  be  practical  for  computer 


When  the  director  cone  of  hand  orientation  is  a right  circular  cone,  then  the 
permissible  range  of  the  hand  can  be  represented  by  the  semi-vertex  angle,  D,  and 
a unit  vector  along  the  axis  drawn  from  the  vertex  to  the  center  of  the  base  circle, 
g.  The  equation  of  the  radical  plane  may  be  expressed  in  terms  of  the  Plucker 
coordinates: 

",  " C 5 : ®o  ) 

- (S„  S,,  S,;  S0)  (2-16) 

where  S„  = - R£  - (*J  + y « + *5).  The  equation  of  the  circle  C.  is  given  by 

radical  plane,  or  the  height  of  the  cone  of  orientation  can  be  determined  by 


S 

2 = sgnOO-jjj- 

-sgn(h)[x,/R«.yI/R„zI/Rc]. 

When  h = 0,  the  cone  of  orientation  degenerates  to 
the  end  effector  is  at  the  maximum  reach.  Let  Pc  deno 
center  of  the  base  circle  Q,  The  components  of  this  i 
L - h/R,(xc,y„2j. 


(2-17) 


(2-18) 

i point,  which  implies  that 
e the  position  vector  to  the 
ector  are  given  by 

(2-19) 


Also,  the  radius  of  the 


: of  orie 


s-'dhl/a^J. 


(2-21) 

the  orientational  capability  of  the  hand  can  be  represented  in  the  following  form: 
CON(g,  fi)  - { a,...,  I cosily,  cosQ0wc, },  0 s Q s it,  (2-22) 

where  0 = 0,.^,),  and  and  0upper  are  the  lower  and  the  upper  limits 

of  0.  For  instance,  CON(g,  0)  with  0 = (0,  it)  is  the  set  of  all  the  unit  vectors 

0,  the  sphere  of  hand  orientation,  s„  intersects  the  hole.  This  representation 
scheme  is  valid  for  a TRS  robot.  It  is  also  useful  for  the  PUMA  robot  as  long  as 
the  sphere  of  hand  orientation  does  not  intersect  the  inner  cylindrical  hole. 
However,  if  the  sphere  of  hand  orientation  of  the  PUMA  robot  intersects  the 
cylindrical  hole,  the  representation  of  the  permissible  hand  orientation  is  not  so 
simple  since  the  directrix  of  the  cone  becomes,  in  general,  a space  quartic. 

2,4  The  Geometric  Capabilities  of  TRS  and  PUMA  Robots 
TRS-type  and  PUMA-type  robots  have  common  geometrical  characteristics 
such  as  1)  the  first  two  turning  joint  axes  intersect  at  right  angles  and  2)  the  second 
and  third  joint  axes  are  parallel.  The  geometric  capability  of  these  types  of  robots 
may  be  measured  by  the  angular  ranges  of  each  of  the  joints  for  the  corresponding 
6RS  spatial  mechanism  which  can  be  constructed  by  connecting  a hypothetical  link 
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mechanisms  for  given  end  effector  positions.  As  described  in  the  previous  section, 
the  first  step  in  this  analysis  was  to  determine  the  permissible  orientation  of  the 
hand.  Utilizing  the  information  of  the  hand  orientation,  this  section  completes  the 
geometric  capability  analysis  for  the  regional  structures  of  the  robots.  As  illustrated 
in  Figure  2.10,  the  geometric  capability  of  a robot  at  a specific  end  effector  position 
may  be  visualized  as  the  closed  regions  both  on  the  workspace  Wtl0  and  on  the 
sphere  of  hand  orientation  s»  within  which  the  points  Pem  and  P„  can  be  respectively 
located.  At  any  instant,  the  points  P,*  and  P„  are  on  the  same  profile  plane.  In 
the  next  section,  determination  of  the  permissible  angular  ranges  for  the  first  joint 
will  be  described.  Discussion  of  the  forearm  and  the  upper  arm  will  follow. 

Having  obtained  the  director  cone  of  hand  orientation,  the  permissible 
angular  range  of  d„  #{,  can  be  represented  by  the  angle  between  two  profile  planes 
tangential  to  the  base  circle  of  the  cone.  The  procedure  of  determining  the 
permissible  ranges,  for  the  two  different  types  of  robots  are  described 


of  the  circle  C » onto  the  X-Y  plane  of  the  fixed  frame,  thus  forming  an  ellipse. 
The  permissible  range,  denoted  by  df,  is  the  angle  made  by  two  tangential  lines  to 
this  ellipse  passing  through  the  origin.  The  angle  T made  by  the  radical  plane  and 
the  X-Y  plane  may  be  given  by 


determined  by  projection 


(2-23) 
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Figure  2.10  Geometric  Capability  of  a Robot 

at  a Specified  End  Effector  Position 


X-Y  plan 


origin  of  a local  frame  with  u-v  coordinates  (see  Figure  2.1 1)  is  placed  at  the  center 
of  the  ellipse,  the  equation  of  the  ellipse  may  be  written  as 

qfu2  + v1  - (wf  = 0 (2-24) 

In  Figure  2.11,  the  angular  range,  df,  made  by  two  tangential  lines  to  the  ellipse 
passing  through  the  point  A can  be  determined  in  the  uv-coordinate  system. 

(*!  = * - 2tari'[(h/RJ!(x2  + y2)  - nil*  (2-25) 

Therefore,  for  any  specified  position  of  the  end  effector,  Pe,  the  permissible 
angular  range  of  t,  can  be  expressed  in  the  form: 

*\-*M 2 S *,  S + t\/2  (2-26) 

65  = tan‘(ye/xe). 

PUMA  Robot.  In  this  case,  the  permissible  range  can  be  measured  by  the 
angle  between  two  common  tangential  lines  to  both  the  ellipse  (i.e.  the  projection 
of  C.)  and  the  XY  projection  of  the  hole,  i.e,  a circle.  In  Figure  2.12,  the 
measurement  of  this  angular  range,  #{,  is  illustrated.  It  is  noted  that  there  could 
be  four  common  tangential  lines  to  both  the  circle  and  the  ellipse.  Two  of  the 
common  tangential  lines  are  symmetric  to  the  others  with  respect  to  the  line  joining 
the  centers  of  the  circle  and  the  ellipse.  The  equations  of  the  ellipse  and  the  circle 

ntf  + v1  - (rjlJ2  = 0.  (2-27) 

u!  + (v  - q)J  - Sa2  = 0,  (2-28) 

terms  of  u and  v coordinates,  where 


respectively,  ic 
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Figure  2.1 1 Determination  of  Permissible  Range  of  $1  for  a TRS  Robot 
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Figure  2.12  Determination  of  Permissible  Range  of  4,  for  the  PUMA  Robot 


(2-29) 


ISol 


The  equation  of  the  common  tangential  line  may  be  given  by 

Thus,  examining  the  condition  that  the  line  should  touch  both 
ellipse  yields 


(2-30) 
and  the 


(2-31) 


Solving  these  equations  for  n gives 

_ -qrj  t t&jq2  * d - - Sj))* 

rJ-S^ 


(2-32) 


(2-33) 


Let  n,  and  n2  be  the  two  values  of  n with  n,  > n2.  And  let  m{  = f(n,)  for  i = X,  2. 
When  re  = S^,  n = «.  Thus,  the  permissible  angular  range  is  given  by 

*\  = */2  - tan',(|m,|).  (2-34) 

When  rt  t S^,  the  permissible  angular  range  becomes 

t‘  = sgn(S22-rc)tan-1(|m2|)-tan-'(|m1|).  (2-35) 

Therefore,  for  given  end  effector  position,  P,,  the  permissible  angular  range  of  d, 
is  given  by 


+ tan'dmj)  s «, 


(2-36) 


(2-37) 


Projection  of  Ihe  Hand  on  a Profile  Plane  The  permissible  orientations  of 
the  upper  arm  and  the  forearm  can  be  determined  by  first  choosing  the  value  of 
that  is,  on  the  corresponding  profile  plane,  or  X,-Z,  plane.  When  the  profile  plane 
is  chosen  for  any  particular  value  of  it  cuts  the  workspace  s„  to  make  a circle 
on  the  profile  plane.  Since  the  wrist  center  can  rotate  around  this  circle  by  some 
angle,  the  hand  may  be  orthogonally  projected  onto  the  profile  plane  so  as  to  obtain 
a planar  3R  robot  on  that  plane.  Figure  2.13  illustrates  this  projection.  Therefore, 
the  analysis  of  geometric  capability  of  the  upper  arm  and  the  forearm  becomes 
identical  to  that  of  the  corresponding  planar  4-bar  mechanism  on  the  profile  plane. 
Once  d,  is  specified,  each  of  the  turning  joints  of  the  corresponding  4-bar 
mechanism  are  labeled  sequentially  from  the  shoulder  joint  Q2,  Qj,  0,  and  Qs  as 
shown  in  Figure  2.13  to  illustrate  notation  conventions  for  the  mechanism 
parameters  used  in  this  section.  In  Figure  2.13,  the  hypothetical  link  a,,  completes 

of  the  end  effector.  It  is  then  recognized  that  the  permissible  angular  ranges  of  each 
of  the  turning  joints  depend  only  on  the  projected  position  of  the  end  effector. 

It  is  reasonable  to  start  the  analysis  by  determining  the  position  of  the  point 
Ps  and  the  length  of  the  projected  hand,  a4i,  on  the  profile  plane  corresponding  to 
any  particular  value  of  tf ,.  The  projected  length  of  the  hand  on  the  profile  plane  is 
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Figure  2.13  Projection  of  the  Hand  on  a Profile  Plane 
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the  radius  of  the  circle,  i.e.,  a«,  which  is  the  intersection  of  s»  and  a profile  plane 
for  given  position  of  the  end  effector.  When  a perpendicular  line  is  dropped  from 
the  center  of  s.  onto  a profile  plane,  the  intersection  point  becomes  the  point  Ps 
in  Figure  Z13.  It  is  noted  that  the  unit  normal  vector  to  a profile  plane  is  identical 
to  §2  at  a specified  value  of  The  radius  of  the  circle,  a^,  can  be  determined 
by 

a«  = [a»J-  KISj-P.1  -Sjj)|2]1'5.  (2-38) 

where  S^  is  an  offset  in  the  shoulder  (S^  = 0 for  TRS-type  robots).  Now  let  P,  be 
the  position  vector  to  the  intersection  point  given  in  terms  of  fixed  frame 
coordinates.  The  perpendicular  line  dropped  from  the  center  of  s.  onto  the  profile 
plane  may  be  given  by 

P3xS!  = Sa:.(PcxS!)  (2-39) 

where  Pe  is  the  position  vector  to  the  reference  point  of  the  end  effector.  Since  the 
equation  of  the  profile  plane  can  be  expressed  by 

Pj  • Sj  - s*  (2-40) 

solving  these  two  equations  for  the  position  vector  P;  yields 

El  “ P«  - (h  ■ E*  )§2  + (2-41) 

It  is  then  noted  that  Sj  = 0 for  a TRS  robot  and  Sj  = Sa  for  the  PUMA  robot. 
Since  the  permissible  angular  range  is  solely  a function  of  the  distance  to  the 
reference  point,  Pt,  of  the  end  effector,  a4!  is,  therefore,  only  a function  of 
when  the  distance  to  the  point  Pc  remains  unchanged. 

Hts-Canstraini  Circles  on  a Profile  Plane.  The  reachable  workspaces  of 
each  of  the  point  Q3  (the  elbow  point)  and  0,  (the  wrist  center,  i.e.,  Pw)  can  be 
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determined  by  the  six  concentric  circles,  or  the  constraint  circles,  on  a profile 
plane.  The  inner  and  the  outer  constraint  circles  are  the  inner  and  outer  reachable 
workspace  boundaries  of  the  corresponding  points  with  the  end  effector  at  any 
specified  position.  When  a position  of  the  end  effector  is  specified  on  the  profile 
plane,  the  six  constraint  circles,  in  general,  can  be  constructed.  Let  C„,  and  C„0 
denote  the  inner  and  the  outer  constraint  circles  of  point  Q„  respectively,  for  n = 
3, 4.  Also,  let  Cj  and  C4  denote  the  constraint  circles  of  joint  Q3  with  respect  to  the 
first  turning  joint  and  joint  04  with  respect  to  the  end  effector,  respectively  (see 
Figure  2.14).  For  given  mechanism  dimensions,  the  radii  of  the  constraint  circles 
are  given  as  shown  in  Table  2.1. 

These  constraint  circles  can  be  best  expressed  in  terms  of  the  first  local 
coordinate  system  as  shown  in  Figure  2.15.  The  coordinates  of  the  point  Pj  given 
by  Eq.  (2-41)  can  be  transformed  into  those  in  the  second  local  coordinate  system': 
Qs  = A,n  P5  (2-42) 

where  is  a transformation  matrix  from  the  i“  to  j"1  coordinate  systems. 

Now  let  us  consider  the  line  of  centers,  i.e.,  the  line  joining  the  centers  of 
two  groups  of  concentric  circles,  and  place  a Xi’-Z,’  coordinate  system  which  is 
rotated  about  the  Y,  axis  by  the  angle  p from  the  X,-Z,  frame  (see  Figure  2.15). 
The  permissible  angular  ranges  of  links  aB  and  a*  can  be  determined  by  identifying 
the  intersections  of  proper  constraint  circles.  It  is  obvious  that  the  intersection 
points  of  the  constraint  circles  are  symmetric  with  respect  to  the  line  of  centers.  If 


It  should  be  noticed  that  the  symbol  Q is  used  to  denote  the  center  point  of  a joint 
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Figure  2.14  Constraint  Circles 


Figure  2.15  Permissible  Angular  Range  of  the  Upper  Arm 


Table:  2.1  Constraint  Circles 


Constraint  Circles 

Radius 

CM(z)  - 0 

r«  = I®b  * as*l 

CJ.z)  =0 

rs  = % 

C Jiz)  = 0 

r4o  = “23  + ®J4 

C,(z)  = 0 

r«  = “45 

C „(a)  =0 

T„  - |aM  - a4)| 

C*(*)  =0 

rlo  = %«  + »4S 

we  let  the  point  (x,z)  in  the  X,'-Z,‘  coordinate  system  correspond  to  the  complex 
number  z - a + ij5,  and  the  point  be  the  intersection  of  two  constraint  circles, 
then  the  point  represented  by  its  conjugate  is  another  intersection  point 

The  Permissible  Angular  Range  of  the  .Upper  Arm-  The  permissible  angular 
range  of  the  upper  arm  can  be  determined  by  intersection  of  the  constraint  circles 
at  points  Cj  and  C,,  (and  also  C,  and  CjJ.  In  the  complex  plane,  i.e„  X,'-Z,’ 
plane,  the  equations  of  these  circles  are  given  by 

Cj(z  ) ■ b - lg  = 0,  (2-43) 

C*(*  )=  (i-isXi  - h)  - (*tj4  - a„)!  - 0,  (2-44) 

C (a  - Zj)(i  - *j)  - * ««s)J  " 0,  (2-45) 

where  zs  is  the  complex  number  corresponding  to  the  point  Qs,  i.e.  z5  = (Q5|  + iO. 
Firstly,  two  intersection  points  of  Cj(z  )and  Cv(r  )can  be  represented  by  a pair  of 
complex  conjugates  in  the  complex  plane  (X/-Z,'  coordinate  system).  By  letting 


z„  (=  0o  + i$0)  denote  the  intersection  point, 
' (»w  + a«)2  + 


I,  therefore,  we  get 


*[(2arfs)J  - {ar!  - (a*  + a„)2  + zf}2]* 


we  get  z,  as  follows: 
“a  - (««  - a«)2  + zf 


*[(2arfs)! ' - (a*  - a0)!  + z2)2]* 


(2-49) 


Since  the  angle  p is  given  by  tan'^Q^Qj  J,  the  permissible  orientation  of  the  upper 
arm  is  determined  by 

t + arg(Zi)  sl,s  p + arg(z„)  (2-50a) 

•P  - arg(z,)  s »j  s -p-  arg (z,)  (2-50b) 

where  arg(z,)  = tan"'(|ft|/«|)  and  arg(zj  - lari'dftj/aj. 

range  of  the  forearm  can  be  determined  either  by  first  specifying  the  orientation  of 
the  upper  arm  or  by  specifying  the  orientation  of  the  hand.  In  this  analysis,  the 
latter  approach  is  taken,  which  implies  that  the  position  of  Q4  is  prespecified. 
Figure  2.16  is  used  to  illustrate  how  to  determine  the  permissible  orientation  of  the 
forearm  for  given  position  of  the  point  Q,.  As  in  the  previous  case,  we  may  place 
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Figure  2.16  Permissible  Angular  Range  of  Ihe  Forearm 


ihe  origin  of  the  X,'-Z,’  i 
through  the  point  Q4.  In 
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coordinate  system  at  the  point  Q2  and  let  the  X,’  axis  pass 
t the  complex  plane  (X^-Z,'  plane),  the  constraint  circles 


C, (x ) - zz  - a^  = 0,  (2-43) 

D , (*)-  (z  - z,)(z  -z,)-*£  = 0,  (2-51) 

where  z,  is  the  complex  number  corresponding  to  the  point  Q4,  i.e..,  zt  - { |Q4| 
+ iO.  Letting  z,  (=  o,  + i|^|,)  denote  the  intersection  point,  the  real  part  of  z,  is 
given  by 


2 z4 


(2-52) 


i[(2arf4)J  - (aj  - a^  + cj)!f 

’ 2?. 


(2-53) 


Therefore,  the  permissible  orientations  of  the  forearm  (94)  are  two  points  given  by 
= i(P  + argfe-z,))  (2-54) 

for  a specified  position  04  where  p - tan'‘(Q4yQ4l). 


2.5  Permissible  Orientations  of  the  Forearm  for  a Contact  with  an  Object 
In  this  section,  permissible  orientations  of  the  forearm  are  considered  for  the 
special  case  that  the  forearm  has  to  maintain  a contact  with  a line  segment  P0P, 
given  in  the  fixed  coordinate  system.  Using  the  same  notation  as  that  in  the 
previous  section,  T0T,  is  used  to  denote  the  line  segment  in  X,-Y,-Z,  coordinate 
system.  Figure  2.17  illustrates  the  constraint  that  the  forearm  movement  of  a robot 
is  subject  to.  In  this  figure,  let  us  suppose  that  the  line  segment  1 has  two  sides  and 


Inverted  Slider— Crank  Mechanism 


Figure  2.17  Orientation  Analysis  of  the  Forearm 
for  Point  Contact  with  an  Object 


the  point  contact  is  restricted  to  the  upper  side.  Under  such  geometric  constraint, 
the  forearm  may  make  a contact  with  line  segment  I either  at  point  T„  or  T,,  or 

by  its  two  end  points,  the  permissible  forearm  orientation  can  be  divided  into  three 
ranges.  The  permissible  orientations  of  the  forearm  can  be  immediately  computed 
from  Figure  2.17.  In  range  1 and  range  2 in  Figure  2.18,  the  regional  structure 
can  be  modeled  by  an  inverted  slide-crank  4-bar  mechanism  with  the  turning  slide 
joint  at  the  points  T0  and  T,  respectively.  However,  only  the  permissible 
orientations  of  the  forearm  are  of  interest  here.  For  consistent  notation  convention, 
the  wrist  point  will  be  considered  as  a reference  point  for  the  forearm  throughout 

determined  by  computing  three  critical  orientations  of  the  forearm,  i.e.; 

2)  orientation  of  the  line  segment  I with  its  reference  point  at  T„ 

Thus,  from  Eq.  (2-54),  orientation  1)  can  be  given  by 

®4*do)  = Po  + arg(z,  - i, ) for  elbow-up  configuration,  (2-55a) 
®<"(To)  = -ft  ~ afgfo  ~ zT(j)  for  elbow-down  configuration,  (2-55b) 
and  also  the  orientations  2)  and  3)  can  be  determined  by 

»;doIt)  = arg^-z^  (2-56) 


•XT.)  - P,  + arg(z,  - z^ 


(2-57) 


f 


:arm  Orientation 


Figure  2.18  Permissible  Ranges  of  the  Fore 
for  Point  Contact  with  an  Objec 


(2-58) 


respectively,  where  z,  (=  a,  + i|8|j  is  given  by 

" . 

2*r, 

‘[(2^t,)!  - (^  - * ttff 

f.  = 1 (2-59) 

and  p,  = tan'1(Tn/TII)  for  i - 0, 1.  Therefore,  the  permissible  range  of  the  forearm 
under  the  given  constraint  can  be  written  as 

range  1:  *«(XoXt)  S K s j;-(To),  (2-60a) 

range  2:  C(T„)  s S',  s tfJ(X,)  (2-60b) 

range  3:  «,(T,)  i S',  s SjfXoXi)-  (2-60c) 


CHAPTER  3 

THE  GEOMETRY  OF  INTERFERENCE 
3.1  Background 

It  is  recognized  that  the  representation  of  the  displacement  of  a moving  frame 
in  Euclidean  3-space  (E3)  is  important  in  the  sense  that  the  notion  of  interference 
between  any  objects  is  associated  with  the  representation.  In  general,  a displacement 
in  E3  can  be  represented  by  several  different  forms  such  as  screw  parameters,  Euler 
parameters,  Eulerian  angles  and  bi-quaternions  [48].  Here,  it  should  be  noted  that 
such  representations  are  described  in  E3  by  means  of  an  orthogonal  coordinate 
system.  When  the  problem  of  interference  between  any  objects  becomes  important, 

describe  only  the  displacement  of  one  coordinate  frame  with  respect  to  another, 
rather  than  that  of  any  definite  object  in  the  coordinate  frame  with  respect  to 
another.  Therefore,  the  goal  in  this  chapter  is  to  develop  a mathematical  tool 
which  provides  an  elegant  way  to  simply  describe  the  condition  of  geometrical 
interference  between  two  objects.  The  types  of  objects  dealt  with  arc  a line  segment 

It  is  recognized  that  the  relative  location  of  two  objects  can  be  represented 
by  an  affine  transformation  in  Euclidean  space.  A displacement  in  a plane  (E2)  can 
be  determined  by  three  numbers,  i.e.  the  position  and  orientation.  This  implies  that 
the  displacement  in  a plane  may  be  mapped  into  a point  in  3-space.  When  the 
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ople,  is 


ned  by  an  affin 


transformation  in  E!,  it  can  be  determined  by  three  parameters  (line  length  is  not 
considered  a parameter  here).  Thus,  there  may  be  a mapping  which  transforms 
these  three  parameters  into  a point  in  3-space.  Let  f:  denote  this  mapping.  Then, 
we  may  write  the  mapping  f:  as  follows: 

f: 

D(d,c) . P(s) 

where  d and  c denote  a relative  position  and  orientation  respectively,  and  s = 
[SpS;, ...  s„)'  is  used  to  describe  the  coordinates  in  a mapping  space.  The  number  n 
is  determined  by  the  number  of  independent  parameters  necessary  to  completely 
describe  the  relative  location  of  two  objects  in  either  E!  or  E5.  For  example,  when 
two  line  segments  are  given  in  Es,  Eve  independent  parameters  are  needed  for 
relative  displacement,  i.e.  d = [x,  y,  z]1  and  c = [c„  cj',  which  means  that  the 
relative  location  of  two  line  segments  can  be  represented  by  a point  in  5-space. 

Therefore,  it  becomes  dear  that  the  objective  of  this  chapter  is  to  develop 
the  mapping  (f:)  which  transforms  a set  of  the  relative  locations  of  two  objects  into 

in  a robot  motion  planning  process,  the  geometric  manifold  representing  the  set  of 
all  the  relative  displacements  of  intersection  is  called  an  image  obstacle.  In  the  case 
of  dealing  with  line  and  space  segments  in  E’,  if  the  parameters  (c's)  are  known, 
or  can  be  prespecified,  then  image  obstacles  become  so  simple  in  their  geometrical 
shapes  that  the  computational  burden  of  computer  representation  of  those  geometric 
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conditions  of  intersection  can  be  significantly  reduced.  Figure  3.1  is  used  to  illustrate 
the  concept  of  this  mapping. 

3.2  Intersection  of  Simolexes 
3.2.1  Representation  of  a Simplex 

A vector  to  an  arbitrary  point  in  E3  can  be  described  by  the  following  equation: 
3 

P = 2 t£  (3-1) 

i=0 

The  four  numbers  1^,  t,.  t,,  and  t3  are  called  the  "barycentric  coordinates"  of  P.  The 
normalized  coordinates  are  called  "volume  coordinates"  (or  "areal  coordinates"  if 
dealing  with  a plane).  From  the  above  expression,  the  point  P can  be  interpreted 
as  the  centroid  (i.e.,  barycentric)  of  the  weighted  points  IjPo,  t,P„  t2P2  and  t}Pj.  The 
point  P is  a convex  combination  of  the  P,  if  all  barycentric  coordinates  are  non- 
negative. 

Now  let  P„,  P„  P2,  and  Pj  be  points  in  E3  not  all  in  the  same  hyperplane  and 
let  P„  P,.  Pj,  and  P3  denote  the  position  vectors  to  the  points  P|  from  the  origin  of 
a fixed  coordinate  system.  Then  the  set  of  all  points  represented  by 

p - Eo  + S tiffi  ■ Eo)  (3-2) 

i=l 

with  q i 0,  for  i = 1,  2,  3,  and  2 q s 1,  is  called  a simplex  with  vertices  P„.  P„  P2, 
and  Pj.  A simplex  is  a convex  polyhedron  which  can  be  represented  by  the 
intersection  of  n + 1 halfspaces.  Using  Eq.  (3-2),  it  is  easy  to  represent  any  interior 
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mapping 


Euclidean  Higher  Dimensional 

3-Space  Mapping  Space 


I - the  set  of  displacements  representing 


Figure  3.1  Mapping  of  Relative  Displacement 


points  of  a convex  polyhedron  in  space.  For  example,  a box  can  be  represented  by 
Eq.  (3-2)  with  the  inequalities  0 < t,  s 1,  for  i = 1,  2,  3.  Figure  3.2  shows  three 
types  of  volumes  that  can  be  represented  by  such  inequalities.  In  a matrix  form,  we 

P - Po  + AT  (3-3) 

where  A is  not,  in  general,  an  orthogonal  matrix,  and  T = [tlv  t^  tl]!.  This 
equation  implies  an  affine  transformation  which  is  a one-to-one  mapping  of  an 
affine  space  onto  itself.  Considering  this  transformation  together  with  constraints  on 
the  parameter  t,  it  helps  us  see  that  the  mapping  expressed  by  Eq.  (3-3)  transforms 
a reference  tetrahedron  into  a homogeneously  deformed  one. 

Now  suppose  we  have  two  space-segments  in  Figure  3.3,  or  tetrahedra,  then 


from  Eq.  (3-3)  we  have 

P = P„  + AT  and  Q = Q0  + BS,  (3-4) 

where  T = [t„  t*  t,]'  and  S = [s„  s*  sj'  with  q,  s„  i 0,  for  i - 1, 2, 3,  and  Xt,, 

2s,  < 1.  The  second  equation  may  be  rewritten  as 

S - B-'(Q  - O0).  (3-5) 

Substituting  the  first  equation  into  the  above  one,  i.eM  setting  P = Q,  we  obtain 
S = B-‘(AT+  de).  (3-6) 


where,  d,,  = P0  - Q0.  This  is  the  (affine)  mapping  which  transforms  the  second 
space  segment  in  the  T coordinate  system  into  one  in  the  S coordinate  system.  In 
other  words,  the  parameters  s,  arc  written  in  terms  of  the  parameters  t,,  or 
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0 < t,  < 1 
0 < te  < 1 
0 < t3  < 1 
tl+tg+t3<l 


0 < t,  < 1 
0 < ta  < 1 
0 < t3  < l 
t,+t3<l 


t3 


Figure  3.2  Primitive  Volumes  represented  by  Inequalities 


0 < ts  < 1 0 < % < 1 

0 < t3  s 1 0 i h < 1 

ti'-tj+tjSi  Si*S5*Sj<i 


Figure  3.3  Intersection  of  Two  Space  Segments 


(3-7) 


B'A  = 


(3-8) 


B-'de  = (d„  d*  d,)'.  (3-9) 

Therefore,  the  intersection  of  two  space  segments  can  be  checked  by  examining  the 

those  of  t,  and  2 1,  for  the  constraints  of  the  s,.  This  method  can  be  utilized  not  only 
to  check  the  static  interference  of  objects  modeled  by  simplexes,  but  also  to 
examine  the  interference  of  a robot  arm  with  obstacles  at  any  instant  in  the  robot 
planning  process.  Since  dg  can  also  be  represented  in  terms  of  affine  (S) 
coordinates,  Eq.  (3-6)  may  be  rewritten  as  follows: 

ds  = A^dg,  (3-10) 


from  Eq.  (3-6)  we  get 

S - B-'A(T  + dj ). 


(3-11) 


interfering  with  a line  segment.  The 


the  space  segment  are  given  by 


P0  = (2,2,0),  P,  = (5,3,0),  P2  = (3,5,0),  P,  = (3,3,3) 

Qo  = (2.  1,-2),  Q,  = (4,4,3). 

From  Eq.  (3-7),  we  get 


Thus,  we  have 

-2500  s (s,  = -(.0833)t  + (.2917))  s .2917 

-.2083  s (sj  = (.9167)t  - (.4167))  s .2500 

-.6667  S (S3  = (1.333)1  - (.6667))  s 1.000 

(2s,  = (1.1818)t  - (.5833))  s 1.5000. 

Since  all  the  parameters  s,  and  2s,  have  intersections  within  the  ranges  0 s s,  s 1 
for  the  constraint  0 s t S 1,  this  implies  that  these  two  objects  are  interfering  with 
one  another.  When  the  parameter  t is  written  in  terms  of  the  s,,  the  intersection 

-17.0000  s (t  = -(24.0000)5,  + (7.0000))  S 7.0000 


.4545  s (t  = (2.1818)s,  + (.4545))  S 2.6364 

.4000  s (t  = (.6000)53  + (.4000))  S 1.000 


Figure  3.4  Examples 


+ (.2800))  i .7600. 


lection  of  these  inequalities 


.4545  s 


; .7600. 


Therefore,  the  intersection  points  can  be  obtained  by 

P„„  = Q„  + (Q,  - Q0)'(.4545)  = (2.9090,  2.3635,  .2725) 
Pml2  = Q0  + (Q,  - Q0)*(.7600)  = (3.52,  3.28,  1.80). 

coordinates  of  their  vertices: 


P0  = (0,0,0),  P,  = (3,0,0),  P2  = (0,2,0),  P3  - (2,0,5) 
Qo  - (5, 1,  0),  Q,  = (7, 1,  0),  Q2  - (5,  3,  0),  Q,  = (5,  1,  7). 
From  Eq.  (3-7),  we  get 


3 0 2 1 r s,  1 [7-5  5-5  5-5 

0 2 0 s,  = 1-1  3-1  1-1 

0 0 5 J [S3  J [fr-0  0-0  7-0 


Thus,  the  parameters  s,  can  be  expressed  in  terms  of  the  t,  as 
.7333  s (s,  = (.6667)1,  - (.9333)1,  + (1.6667))  S 1.0000 
.5000  s (Sj  = (1.0000)1,  + (.5000))  s 1.0000 
.0000  S (S3  = (1.2000)1,  s 1.0000 


.0000  s (t,  = (1.5000)5,  + (1.0000)5,  - (2.5000))  s .0000 
.0000  S (t,  = (l.OOOO)s,  - (.5000))  s .5000 
.0000  s (t,  = (.7143)Sj  s .7143 

which  implies  that  these  two  objects  are  interfering  with  one  another. 


2.3.  Macping-oLGtgmtlrital  Interference 


Let  us  first  consider  two  line  segments  in  £!.  In  Figure  3.5,  the  symbol  2 is 
used  to  denote  a fixed  frame  while  e denotes  a moving  frame.  Suppose  that  the  two 
line  segments  lie  on  the  x axes  of  the  moving  frames  respectively.  The  displacement 


transformation.  Let  p,  and  p,  be  the  unit  vectors  in  2 along  two  line  segments 
respectively,  and  also  let  a,  and  a,  denote  the  lengths  of  the  line  segments.  Then, 


£i  * [ l*i.  ]'.  it  ■ [ U “a  ]'•  (3-12) 

The  position  vector  P„'  to  the  end  point  of  the  line  segment  can  be  given  either 


by 


P0’  = EPe  + P0  (3-13) 

where  E is  an  orthogonal  matrix,  or  by 

P0’  = AP*  + P0  (3-14) 

where  A is  not,  in  general  an  orthogonal  matrix,  and  Pg  and  P,,  are  the  position 
vectors  measured  in  the  moving  frames  fE  and  cA  respectively.  The  line  segment 
a,p,  is  denoted  as  a reference  line  segment  and  the  point  P„’  is  denoted  as  a 
reference  point.  What  we  would  like  to  do,  at  this  stage,  is  to  construct  an  affine 

in  which  column  vectors  g,  and  g,  are  in  order.  Hence,  the  vector  P*  measured  in 


of  the  affix] 
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Figure  3.5  Repr 


of  a Displacement  by  use  of  Affine  Transformation 


P*  = A-'EPe  = dpe 
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(3-15) 


(3-16) 


(3-17) 


Let  s,  and  s,  denote  the  affine  coordinates  of  in  eA.  Then,  from  Eq.  (3-15),  we 
may  write 


l1rlra«2l  = 

I m,,  m^ 

Thus,  we  get 

D 1 T UalnJ  -4iU-n>»imd 
lla”*!  [ 0 1 

Therefore,  the  affine  coordinates  of  PA  are  given  by 
ftilj  + 


(3-18) 

(3-19) 


(3-20) 


(3-21a) 


% = — n j—  y 

in  terms  of  the  direction  cosines  of  two  line  segments  and  the  relative  position  of  a 
reference  point  measured  in  a fixed  frame.  By  introducing  a new  parameter  such 
that 

Sj  = tan(ii-ii- ) , (3-21b) 

the  mapping  is  then  completed.  When  the  affine  coordinates  are  used  to  represent 
the  point  in  a moving  plane  eA,  the  direction  cosines  may  be  written  such  that  1(, 
= niy,  = 1.0  and  lyl  - mxl  = 0.0.  Thus,  we  get 


D 


T 1 -la/mjjl 

L 0 l/m„J 


(3-22) 


and  therefore. 

s,  = x - (l„/m2I)y 

h = (l/nta)y 


(3-23a) 


Sj  = tan(£j,/2).  (3-23b) 

It  should  be  noted  here  that  the  subscript  21  denotes  the  measurement  with  respect 
to  the  orthogonal  coordinate  system  attached  to  the  line  segment  a,g,.  When  two 
line  segments  intersect,  it  becomes  obvious  that  two  parameters  s2  and  s.  must 
satisfy  the  following  conditions  (see  Figure  3.6): 


(3-24) 


iM 


:?r 


Figure  3.6  Mapping  of  a Relative  Planar  Displacement 


i was  expressed  by 


in  3.1  into  consideration  once  agai 


£ 


D(d,  e)  — 

where  d and  c denote  a relative  position  and  orientation  respectively.  Here,  in 
this  case,  we  have  the  following:  d ■ [x,  yj'  and  c = f21.  Thus,  the  relative 

into  an  image  point  in  3-space  by  the  mapping  given  by  Eq.  (3-23).  The  inequalities 

represented  by  an  infinitely  long  quadrilateral  prism  shown  in  Figure  3.6.  In  order 
to  get  a little  more  insight  into  this  mapping,  we  may  use  the  following  relationship: 


J-Sj 


t Sj  = tan({j,/2).  Since  12,  = cosf2. 


i*4 


For  a given  position  (x  and  y)  and  orientation  (s3),  Eq.  (3-23)  is  an  one-to-one 
mapping  in  the  interval  -x  < f 21  < x excluding  f21  = 0.  Thus,  there  exists  a unique 
pair  of  coordinates,  (s„  Sj),  for  each  value  of  Sj.  From  this  observation,  it  is 
possible  to  project  image  points  onto  unique  locations  in  the  s,-Sj  plane  without 
overlapping  two  distinct  image  points.  By  such  projection,  the  inequalities  (3-24) 


quadrangle 


segments  intersect  for  a given  relative  orientation  of  a line  segment  with  respect  to 
the  reference  line  segment. 


3.3.2  Mapping  of  a Relative  Spatial  Displacement 


b . [Ifl.ma.naf 


When  these  three  unit  vectors  are  linearly  independent,  an  affine  coordinate  system 
can  be  constructed  such  that 


Let 


b ■ [■oK^Od]' 

= (sixfit)/  lax&l 


(3-28) 


(3-29) 


from  Eq.  (3-14).  Then,  the  matrix  D can  be  obtained 


as  follows: 


D 


A’1  E 


|l,1mflnd| 


Im^n^l  -|l„jnJ  li^m^l 
-ItHaHal  HaHsl  -ll^m.jl 

I m^nj  -|l,,n,,|  I l,,m^  | 


K,  I,,  In 

m,i  nty,  m11 


(3-30) 
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In  this  case,  as  shown  in  Figure  3.7,  the  orthogonal  matrix  E corresponding  to  the 
reference  line  segment  may  be  obtained  in  such  a way  that  the  reference  line 
segment  lies  on  the  x axis  of  a moving  frame  eE  while  the  line  segment  a,ft  is 
coincident  with  the  x axis  of  eE.  The  unit  vectors  along  the  z axis  (i.e.,  the  3rd 
column  in  E)  and  the  y axis  (i.e.,  the  2nd  column  in  E)  of  eE  are  given  by  ft  and 
4X3  x a,.  Let  e]t  e,,  and  £3  denote  the  orthonormal  set  of  vectors  representing  the 
moving  frame,  fE.  Then,  we  can  write 
e,  = [ 1„,  ift„  ft,  ]'  = ft, 

£2  = [ V- “yt,  nji  1'  = (fc*l*i)/  lifa  x ihl  (3’31) 

b = [ U “zu  ».i  J'  = b 

The  transformation  matrix  D can  be  further  reduced  if  the  relative  displacement  of 

a line  segment  with  respect  to  a reference  line  segment  is  used.  Since  eE  becomes 
a reference  frame,  we  may  write  matrix  D as  follows: 


It  is  then  obvious  that  the  unit  vector  ft  measured  in  the  moving  frame  £E  can  be 
written  as  ft,  = [ 0, 0, 1 ]',  and  also  ft,  = 0.  Substituting  these  back  into  Eq.  (3- 
32)  and  expanding  each  entry  yields 


Figure  3.7  Relative  Displacement  of  Two  Line  Segments 


(3-33) 


Therefore,  the  mapping  that  transforms  a relative  displacement  of  two  line  segments 
in  E3  into  a point  in  a mapping  space  is  now  given  by 
Si  = * " Oa/“a)  y 

Sj  = (l/m,,)y  (3-34a) 

Sj  = -(nj,/mj,)y  + z 

Since  five  independent  parameters  are  needed  in  order  to  specify  the  relative 
displacement  of  a line  segment  in  E3,  this  mapping  can  be  completed  by  introducing 
two  new  parameters  such  that 

s4  = tan(  -y- ) , 

and  (3-34b) 

5»  = tan(  -2!_ ) , 

where  f 2i  and  t>2i  are  the  angles  measured  in  eE  as  illustrated  in  Figure  3.7.  From 
Eq.  (3-34b),  we  have 


(l-sftl-sD 

(1  + s})(i  + 4)' 

2s, (1  - $ 

nij!  - — < 

(1  + S3)(l  + Sj) 

and  (3-35) 
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ig  may  be  rewritten  as 


2*4(1  - *1) 
ss(l  + si) 


^ s«(l-sl) 
and  Ihe  condition  for  interference  of  two  line  segments  in  th 
now  expressed  by  the  inequalities 


-%  S s,  s 0 
s,  = 0 

it  the  boundary  of  thi 


d rectangle  in  Figure  3.7. 


. Let  us  suppose  that  we  have  one  plane 
segment  represented  by  two  vectors  aji,  and  ajg,  together  with  one  line  segment 
ajjt, . In  this  case,  we  may  construct  an  affine  coordinate  system  such  that  the  three 
vectors  form  its  coordinate  axes.  The  special  case  that  three  given  vectors  are 
linearly  dependent  will  be  excluded  here.  The  matrix  A can  be  expressed  in  the 


following  form: 


A*  % ma  mj  (3-38) 

L ^ ”*3  J 

Hero,  in  this  case,  the  orthogonal  matrix  E which  corresponds  to  the  reference  line 
segment  can  be  obtained  in  a manner  similar  to  the  case  of  two  line  segments:  Let 
the  reference  plane  segment  lie  on  the  x-y  plane  of  a moving  frame  eE  and  the  line 
segment  ajh  on  the  x axis  of  cE  as  shown  in  Figure  3.8.  Construct  unit  vectors 
along  the  z and  the  y axes  of  eE  by  p,  x & and  (p,  x (ij)  x p,  in  order.  The  entry 
d,  of  matrix  D is  then  expressed  by  Eq.  (3-30).  Now  let  us  consider  the  relative 
displacement  of  a line  segment  with  respect  to  a reference  plane  segment. 
Rewriting  Eq.  (3-32),  we  have 

lm*tn3il  -|>2t"jil  I I 1 
0 n31  -m,,  (3-32) 

Therefore,  we  obtain  the  complete  mapping  of  a relative  displacement  of  a line 
segment  with  respect  to  a plane  segment: 


I “nnn  I 


(3-39a) 
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Figure  3.8  Relative  Displacement  of  a Line  and  a Plane  Segments 


I m21n31 1 I m21n31  I 

Again,  since  five  independem  parameters  are  needed  in  order  to  completely  specify 
relative  displacement  of  a line  segment  in  EJ,  two  new  parameters  can  be  introduced 
such  that 


where  E31  and  p,,  are  the  angles  obtained  in  tE  by  replacing  the  subscripts  2]  in  Eq. 
(3-34b)  with  M.  These  equations  represent  a complete  mapping  f:  of  the  relative 
displacement  of  a line  segment  with  respect  to  a plane  segment  in  E3  into  a point 
in  a mapping  space.  It  should  be  noted  that  the  relative  measures  of  direcdon 


cosines  of  i.e.,  I2I,  m21,  and  n2I,  are  constants  sin 
segments,  a,^,  and  a^,  is  fixed.  Since  the  twi 
coplanar,  it  becomes  obvious  that  n21  - 0 ant 
Substituting  ttj,  = 0 into  Eq.  (3-39a)  gives 

121  | I;im3l  | 

m2l  m2in3t 


e the  angle  made  by  two  line 
unit  vectors,  p,  and  p,,  are 


(3-40) 


Since  12I  and  mj,  are  fixed  constants  for  a given  plane  segment,  from  Eq.  (3-35), 


0 - s.2)0  - sj) 

(1  + si)(l  + s’) 

2s4(1  - s’) 

(1  + sj)<l  + s’) 


(3-35) 


Therefore,  this  mapping  can  be  rewritten  as 


>2. 


(2lJ1s4-mn)(I-s|) 

z, 

2m2,  Ss  (1  + sj) 


s4  (1  -sf) 
ss  (1  + sj) 


(3-41) 


l + s? 

Sj  = z. 

*5 

The  geometric  conditions  representing  intersection  of  a line  and  a plane  segment 
may  be  written  as  follows: 


0 s s,  s a* 


(3-42) 


s,/a,  + Sj/aj  i 1. 


In  Figure  3.8,  the  shaded  volume  represents  the  geometric  condition  for  intersection 
expressed  by  the  above  inequalities.  Again,  Figure  3.8  shows  the  s,-5j-Sj  subspacc 
of  a mapping  space. 


3.4  Inverse  Mapping 

It  has  been  illustrated  that  an  affine  coordinate  system  can  be  constructed  for 
given  simple  objects  so  that  the  interference  can  be  easily  checked.  Throughout  this 

segment  with  respect  to  a reference  line  segment  or  plane  segment  can  be  mapped 
into  a point  in  the  mapping  space.  Conversely,  it  is  found  that  a set  of  n points  in 
the  mapping  space  corresponds  to  that  of  n finitely  separated  relative  locations  of 
a line  segment  with  respect  to  a reference  line  segment  or  plane  segment. 
Furthermore,  a curve  in  the  mapping  space  is  the  image  of  a continuous  motion  of 
one  object  with  respect  to  a reference  one.  In  this  section,  the  inverse  mapping  of 
the  relative  location  of  objects  in  E3  and  the  correspondence  of  the  relative  motion 
to  the  geometry  in  the  mapping  space  will  be  investigated. 

The  inverse  of  matrix  D can  be  directly  obtained  from  Eq.  (3-15)  as  follows: 
D'1  = E'1  A (3-43) 

displacement  of  two  objects,  two  line  segments,  or  a line  and  a plane  segment,  in 
Euclidean  space.  Let  us  first  consider  two  line  segments  in  a plane.  For  this  case, 
the  inverse  D’1  may  be  then  expressed  by 


Thus,  we  can  write 


x = s,  + lns, 
y = mj,Sj 


(3-45a) 


fa  - tan  ‘(2s,).  (345b) 

Now,  eliminating  direction  cosines  from  the  above  equation  yields 

<s,  - X)2  - 4 * / - 0.  (3-46) 

This  geometric  manifold  can  be  interpreted  as  the  projection  of  a spatial  curve 
expressed  by  Eq.  (3-21)  onto  any  plane  parallel  to  the  s,-Sj  plane  and  is  represented 
by  a hyperbola  on  the  plane  of  projection.  When  the  line  segment  aj/jj  rotates  about 
its  own  end  point  P0  in  Figure  35,  the  image  point  travels  from  one  point  to  another 
along  the  spatial  curve  as  shown  in  Figure  3.9.  The  upper  part  of  a hyperbola 
represents  a set  of  image  points  of  a line  segment  in  the  range  of  0 s {21  s * while 
the  lower  part  represents  that  in  the  range  of  -*  s { S 0. 

For  the  case  of  two  line  segments  in  3-space,  we  can  get  the  inverse  mapping 


from  Eq.  (3-33), 


Figure  3.9  Mapping 


Displacen 


(3-47) 


r i i21  o i 

D'1  = I 0 mj,  0 

[ 0 nj,  1 J 

Thus,  we  get 

x - s,  + l„s, 

y = mj,s,  (3-48a) 

z = njiSj  + Sj 


(„  = tan-‘(2s4) 
*n  - tan  '(2ss). 


(3-49b) 


Eliminating  the  direction  cosines  from  these  equations  yields 

(s,  - x)1  -4  + (s,  - z)2  + y2  - 0 (3-50) 

which  represents  a hyperboloid  of  two  sheets.  This  quadric  in  the  S|-Sj-s3  coordinate 
system  can  be  interpreted  as  a projection  of  a geometric  manifold  onto  the  s,-Sj-S3 
3-flat  which  represents  all  the  possible  relative  locations  of  two  line  segments  (see 
Figure  3.10).  It  is  recognized  that  the  two-dimensional  case  can  be  derived  from 
the  above  equation  by  setting  n2,  = s,  = z = 0. 

Finally,  the  inverse  matrix  D'1  for  the  case  of  a line  segment  and  a plane 
segment  is  obtained  from  Eq.  (3-32): 


Figure  3.10  Mapping  Space  of  a Relative  Spatial  Displacement 
of  a Line  Segment 


0 0 


(3-51) 


Therefore,  we  get 

* = s,  + 1j,Sj  + l3,s, 

y = mj,s,  + mjjSj  (3-52a) 

z = nj,Sj, 

and  also, 

€n  = tan-'(2s4), 

(3-52b) 

l»3i  = tan",(2sJ)- 

(s,  - x)2  + ^ - sj  + 2I21s,Sj  - 2(lj,x  + m^yiSj  + y2  + z2  = 0 (3-53) 

reference  plane  segment  is  chosen,  this  angle  becomes  a constant.  Therefore,  this 
equation  represents  a quadric  in  the  8,-Sj-s,  coordinate  system. 

3.5  Relative  Motions  and  .SupcrimBPSiiiaiLoOmagc  Jtoinis 
So  far,  a mapping  that  transforms  a relative  displacement  of  two  objects  into 
an  image  point  has  been  developed  and  the  geometric  conditions  of  intersection  of 
two  objects  represented  in  a mapping  space  have  been  investigated.  A set  of  n 
finitely  separated  relative  locations  of  a line  segment  with  respect  to  a reference  line 
segment  or  plane  segment  corresponds  to  a set  of  n image  points  in  the  mapping 


is  no  difference  in  between  displacing  a moving  object  with  respect  to  a reference 
one  and  displacing  a reference  object  while  a moving  object  remains  unchanged. 
Let  us  suppose  that  a line  segment  and  a reference  plane  segment  are  given  and  the 
line  segment  is  displaced  into  another  location  with  respect  to  the  reference  plane 
segment.  Then,  we  can  get  two  image  points  which  represent  two  distinct  relative 
locations  of  a line  segment  with  respect  to  a reference  plane  segment 


by 


[Ella,  - (Et  + SUE,- 


(E.Ia,  - a;‘e,  [P,]Ei 
- D,  IE, la. 


(3-55a) 


(EJa,  - Aj'E,  (PJe, 

= D.  [PJ^  (3-55b) 

where  D,  = Aj'E,  for  i - 1,  2.  [P]^  and  [P]^  are  used  to  denote  the  position 
vectors  to  a point  P measured  in  a moving  orthogonal  and  affine  coordinate  systems 
respectively,  and  also.  E,  and  A,  to  denote  a Euclidean  and  a general  non- 
orthogonal  matrix.  Since  the  relative  orientation  of  a line  segment  was  not 
A;1  = Aj\  E,  = Ej  and  D,  = D2.  Thus,  Eq.  (3-54b)  can  be  rewritten  as 
IEJa,  = D,  [P,  + d]E, 

= DtlE.k  + D,  [dL 


t changed. 


= [P.U,  + [d]„ 


(3-56) 


Hl*,“  D,  [d]E|.  (3-57) 

This  equation  implies  that  the  relative  translation  of  a line  segment  with  respect  to 
a reference  one  corresponds  to  a translation  of  an  image  point  into  another  in  a 
mapping  space  (see  Figure  3.11). 

Relative  Rotation.  It  was  shown  that  when  a reference  point  of  a line 
segment  is  rotated  about  its  reference  point,  the  corresponding  image  point  travels 
on  a geometric  manifold  expressed  by  Eq.  (3-46),  (3-50),  or  (3-53)  depending  on  the 
case.  In  order  to  get  a little  insight  into  such  transformation  in  a mapping  space,  we 
may  rewrite  Eq.  (3-15)  as 

(E.Ie,  = E;'A,  [P,]Ai.  (3-58) 

When  a line  segment  is  rotated  to  another  orientation  about  its  reference  point,  a 
new  image  point  may  be  expressed  by 

(PJa,  = A^'E.  [P.Ie, 

= (3-59) 

Substituting  Eq.  (3-58)  into  Eq.  (3-59)  yields 

IEJa,  = Ve,  e;'a,  (P,]Ai 

= V A,  [?,]*_.  (3-60) 

This  implies  that  a relative  rotation  can  be  represented  by  an  affine  transformation 
in  a mapping  space  (see  Figure  3.12). 


Relative  Trar 


Figure  3.11  Relative  Translat 
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Relative  Rotation 
y [(,rv^) 


Figure  3.12  Relative  Rotation 


;mem-  a general  displaceme 

nt  can  be  obtained  by 

a rotation  followed  by  a translat 

ion.  From  Eq.(3-60),  we  hav 

[PJa,  = Elly 

(3-61) 

Lei  (Ej’Ia,  denote  an  image  poir 

it  after  general  displacement. 

Thus,  we  can  write 

[Pj’Ia,-  [PJa,  + BU, 

= Aj'A,  E,1a,  + 

= Aj'A.lP,]^  + 

Aj'EJd]^ 

- Aj'A.IP.U,  + 

Aj'E^'EJd]^ 

= Aj'A.IPjA,  + 

AJ,E1E;1A,[d)Ai. 

Therefore,  we  get 

E1a,=  Aj‘A, «?,]*,  + 

m*,)- 

(3-62) 

; Points.  It  is  obvious  that  t 

here  is  no  difference 

a reference  object  while  a moving  object  remains  unchanged.  For  an  illustrative 
example,  let  us  consider  one  line  and  two  plane  segments.  While  one  plane 
segment  at  a location  A is  taken  as  a reference  plane,  another  plane  segment  at  a 

Although  these  two  plane  segments  have  different  shapes,  the  image  points  of  the 
relative  locations  of  a line  segment  with  respect  to  two  different  reference  plane 


segments  can  be  superimposed  in  the  same  map  by  maintaining  the  information 
about  the  dimensions  of  the  corresponding  image  obstacles.  Figure  3.13  shows  the 


superimposition 


Image  Points 


Figure  3.13  Superimposition  of  Image  Points 


CHAPTER  4 

APPLICATION  TO  OBSTACLE  AVOIDANCE 


4.1  Overview  of  the  Basic  Approach 


The  theory  for  robot  articulation  consists  of  two  independent  ingredients.  i.e., 
the  kinematic  analysis  of  the  geometrical  capability  of  a robot,  and  the  geometiy  of 
interference.  Analysis  of  the  geometrical  capability  of  a robot  involves  a study  of  the 

of  the  joints  for  a specified  position  (or  orientation)  of  the  end  effector.  The  outputs 
from  a study  of  the  geometiy  of  interference  are  computational  algorithms  that  can 
be  served  as  rapid  decision-making  tools  for  various  levels  of  path  planning  process. 

For  a practical  application  of  this  theoty,  these  two  equally  important 
ingredients  can  be  combined  into  a path  planning  software  via  computer 
implementation  of  the  algorithms  and  a proper  technique  for  the  geometric  data 
manipulation.  The  geometric  data  representing  a world  model  can  be  handled  by 
means  of  reliable  data  structures  while  the  computational  algorithms  are  used  as 
decision-making  tools.  Such  decision-making  capabilities  utilizing  robot  geometries 
may  be  characterized  by  the  term,  "Machine  Intelligence  via  Robot  Geometry." 
For  a practical  computer  implementation  of  the  application,  the  PUMA  robot  has 


the  algorithms  developed  in  this  chapter 
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are  applied  to  TRS-type  robots,  they  become  simpler.  This  is  because  of  the  lack 
of  offsets  in  the  shoulders  of  such  robots. 

Remembering  that  the  underlying  assumption  in  the  obstacle  avoidance 
problem  is  that  the  geometric  information  of  the  robot  working  environment  is  given, 
the  basic  approach  to  the  development  of  a collision-free  path  planner  is  now 
described.  The  first  process  in  computer  implementation  of  machine  intelligence  via 
robot  geometry  is  to  store  the  geometric  data  in  the  proper  forms  of  data  structures 
based  on  the  algorithms.  The  second  process  involves  the  analysis  of  the  robot 
working  environment.  The  analysis  of  the  environment  provides  decision-making 
capability  by  suggesting  the  preliminary  strategy  for  the  arm  movement  of  the  robot. 
In  the  third  process  the  workspace  volume  (of  the  PUMA),  bounded  by  certain 
surfaces  corresponding  to  the  starting  and  the  goal  configurations  of  the  robot,  is 
divided  into  many  subsections.  Since  the  robot  motion  along  any  successful  collision- 
free  path  should  be  reversible,  the  planning  process  may  be  carried  out  from  both 
ends,  i.e„  a starting  and  a goal  position.  Employing  this  concept,  the  robot 
workspace  can  be  divided  into  many  subsections  depending  on  the  geometries  of  the 
robot  and  obstacles.  Let  us  suppose  that  the  subdivision  of  the  robot  workspace  is 
completed.  Then,  it  may  be  possible  to  determine  a collision-free  configuration  at 
a temporary  goal  position  in  the  first  subsection.  In  the  same  manner,  while  a goal 
position  is  considered  as  a temporary  starting  position  for  the  last  subsection,  a 
collision-free  configuration  at  a temporary  goal  position  can  be  determined.  This 
process  can  be  continued  towards  the  middle  subsection  so  that  the  collision-free 
configurations  of  the  robot  at  the  boundaries  between  the  subsections  are  first 
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determined.  The  path  planning  may  be  then  separately  performed  in  each  of  the 
subsections,  or  it  may  be  done  in  parallel.  In  the  last  process,  the  geometiy  of 
interference  developed  in  chapter  3 takes  a major  role  in  planning.  Thus,  the  final 
planning  process  is  accomplished  in  the  mapping  space. 


Primitive  Volumes.  As  shown  in  chapter  3,  interior  points  of  three  types 
of  primitive  volumes  (see  Figure  3.2)  can  be  expressed  by  simple  inequalities  of 
affine  coordinates.  When  obstacles  are  modeled  by  simplexes,  the  technique 
developed  in  section  3.1  can  be  employed  to  check  the  intersection  of  stationary 
obstacles  very  efficiently.  Therefore,  it  is  necessary  to  assume  that  obstacles  can  be 
represented  or  approximated  by  one  of  the  primitive  volumes  or  any  combination 
of  them.  Although  it  is  possible  to  deal  with  an  obstacle  modeled  by  a space 
segment  which  does  not  have  a fiat  lop  or  bottom  face,  this  demands  the 
development  of  computational  technique  to  analyze  the  orientation  of  such  an 
obstacle  relative  to  a robot.  For  this  reason,  only  those  obstacles  which  have  flat 
top  or  bottom  faces  will  be  considered  in  this  work.  More  detailed  discussion  on 
these  types  of  obstacles  will  be  given  in  section  4.32. 


modeled  as  a series  of  polyhedra.  In  general,  it  is  not  a simple  task  to  deal  with 
such  a robot  model  together  with  polyhedral  obstacles  in  path  planning.  However, 
it  is  possible  to  homogeneously  enlarge  obstacles  when  all  the  links  of  a robot  can 
be  modeled  by  enveloping  cylinders  and  hemispheres.  In  this  way,  the  links  of  a 


1U. 


-ApproxnnatiQiLof  Obstacles 


S.  For  computational  simplicity,  a robot  can  be 
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robot  can  be  modeled  by  serially  connected  line  segments.  This  technique  has  been 
applied  to  TRS-type  robots  by  many  researchers  for  its  computational  simplicity.  It 
should  be  noted,  however,  that  it  is  inevitable  in  the  use  of  this  technique  to 
sacrifice,  to  a certain  extent,  the  accuracy  in  volume  expansion  for  computational 
simplicity.  For  instance,  when  a polyhedron  is  enlarged  by  a certain  dimension,  the 
expanded  volume  is  not  a polyhedron,  but  a volume  with  rounded  corners.  This 
geometrically  complicated  volume  with  the  rounded  comers  may  be  further 
approximated  by  a polyhedron  which  can  be  formed  by  expanding  the  planar  faces. 

The  approximation  technique  (by  volume  expansion)  reduces  computational 
burden  in  checking  possible  interference  of  robot  links  and  obstacles.  However,  for 
the  cases  of  many  robots,  it  is  not  quite  correct  to  enlarge  obstacles  by  the  same 
radius  as  that  of  the  enveloping  cylinders.  Let  us  consider  the  PUMA  robot.  The 
upper  arm  of  the  PUMA  may  be  modeled  by  an  enveloping  cylinder  with  a certain 
radius.  However,  the  approximation  of  the  upper  arm  and  the  wrist  mechanism 
(including  the  end  effector)  by  enveloping  cylinders  with  the  same  radius  requires 
excessive  sacrifice  in  geometrical  accuracy.  In  order  to  solve  this  problem,  the 

idea  of  the  selective  approximation  is  to  enlarge  obstacles  by  many  different  radii 
of  enclosing  cylinders  of  the  robot  links  and  to  store  all  the  geometric  data 
corresponding  to  the  expanded  obstacles.  This  is  done  in  the  first  process  of  path 
planning  described  in  the  previous  section.  Figure  4.1  shows  the  PUMA  robot 
modeled  by  different  sizes  of  enclosing  cylinders.  The  technique  of  selective 
approximation  allows  the  PUMA  robot  to  be  modeled  by  series  of  line  segments. 
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Figure  4.1  Geometric  Model  of  the  PUMA  Robot 
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Through  the  second  and  third  stages  of  path  planning,  the  planner  provides  decision- 
making capability  of  "selecting"  one  of  the  expanded  obstacle  data  in  each  of  the 
subsections  of  robot  workspace  where  the  planning  can  be  independently  performed. 

4.2  Blueprint  Algorithm 
4,2,1  Taxonomy  of  Robot  Arm  Movcmcms 

Robot  path  planning  involves  the  intersection  checking  of  stationary  and 

to  check  such  intersections.  In  this  chapter,  such  an  efficient  collision-free  path 

geometry  of  interference  takes  a major  role.  The  blueprint  algorithm  is  a rule-based 
planning  algorithm. 

The  observation  of  robot  arm  movement  amongst  obstacles  suggests  that  there 
could  be  three  different  arm  movements  for  a robot  to  avoid  collision  with  obstacles. 

types  of  possible  robot  arm  movements  amongst  obstacles  may  be  classed  into  three 
categories: 

1)  go-over, 

2)  withdraw. 

3)  go-undemcath. 

When  one  obstacle  is  placed  on  the  ground,  it  is  clear  that  a robot  can  go  over 


it.  For 


obstacle  hanging  from  above. 


go  underneath 


withdraw  the  arm.  These  three  types  of  robot  arm  movements  serve  as  basic  rules 
for  a robot  path  planner.  Therefore,  when  geometrical  information  about  one 
obstacle  is  provided  to  a path  planner,  one  of  three  rules  of  arm  movements  will 
be  applied  based  on  its  geometrical  information. 

In  order  to  make  it  possible  to  construct  a 'blueprint'  for  a path  planning,  all 

process  may  be  referred  to  as  geometrical  preprocess.  The  geometrical  preprocess 

1)  expansions  of  obstacles  and  storage  of  geometric  data, 

2)  identification  of  expanded  obstacles  placed  in  robot’s 
workspace  within  the  input  range, 

3)  identification  of  expanded  obstacles  in  Wclb,  W„  and  We. 

4,2,2  Most  Significant  and  Primary  Obstacles 

When  the  input  configurations  of  a robot  at  starting  and  goal  positions  are 

the  simplest  path,  though  not  necessarily  feasible.  This  straight  line  is  referred  to 
here  as  a primary  path.  Although  it  is  obvious  that  a primaty  path  is  the  ’simplest" 
one,  it  is  not  always  a simple  task  to  check  if  it  is  possible  to  achieve  a collision- 
free  motion  along  a primaiy  path.  It  may  involve  complicated  and  time-consuming 
computation.  However,  it  is  relatively  easy  to  check  whether  or  not  a primary  path 
intersects  any  obstacle.  Since  the  straight  line  is  normally  a path  for  the  reference 
point  of  a robot  (for  instance,  the  tip  of  end  effector)  it  is  not 


possible  for  a robot 
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to  carty  out  a collision-free  motion  along  a primary  path  when  it  intersects  any 
obstacle.  It  is  dear  that  when  a robot  moves  along  a primary  path,  the  robot  must 
avoid  collision  with  the  obstacles  intersecting  a primary  path.  Those  obstacles  that 

computational  implementation,  the  result  from  the  intersection  of  simplexes 
presented  in  chapter  2 can  be  utilized.  Thus,  most  significant  obstacles  can  be 
identified  by  examining  the  intersection  of  obstacles  and  a line  segment,  i.e„  a line 
segment  joining  a starting  and  goal  positions.  Figure  4.2  illustrates  the  terminology 
used  in  this  chapter. 

Now  suppose  that  the  hand  of  a robot  is  allowed  to  change  its  orientation 
while  a reference  point  on  the  hand  moves  along  a primary  path.  Since  the 
workspace  of  the  hand  with  respect  to  any  specified  position  of  the  end  effector  is 
a sphere  (the  sphere  of  hand  orientation),  it  sweeps  a cylindrical  volume  enclosed 
by  two  hemispheres  at  both  ends  when  the  end  effector  moves  along  a primary 
(straight  line)  path.  This  swept  volume  may  not  be  a complete  cylindrical  solid  since 
this  volume  can  be  cut  off  by  the  boundary  surfaces  of  the  workspace  of  the  wrist 
center.  This  swept  volume  will  be  referred  to  here  as  a primary  cylinder.  A primary 
cylinder  may  interfere  with  some  obstacles  besides  the  most  significant  obstacles. 
For  the  convenience  of  description,  the  obstacles,  including  the  most  significant 
obstacles,  that  interfere  with  a primary  cylinder  will  be  called  primary  obstacles. 
The  identification  of  primary  obstacles  means  that  the  geometric  data  of  primary 
obstacles  will  be  immediately  available  for  collision-free  planning  of  the  hand  of  a 


robot.  In 


Figure  4.2  Terminology 


a primary  cylinder  and  primary  obstacles. 

Upon  examining  the  geometric  data  of  primary  obstacles,  these  obstacles  can 
be  sorted  by  the  angular  values  of  the  first  joint  corresponding  to  the  profile  planes 
between  the  starting  and  goal  positions.  It  should  be  noted  that  primary  obstacles 
are  those  which  are  enlarged  by  the  radius  of  the  enveloping  cylinder  of  the  robot 


Obstacles  that  are  not  primary,  but  placed  in  the  workspace  of  a wrist  center 
bounded  by  two  profile  planes  at  starting  and  goal  positions  may  be  classed  as 


interfere  with  the  forearm  or/and  the  upper  arm  of  a robot  In  terms  of 
computation,  secondary  obstacles  are  the  geometric  data  associated  with  the 
obstacles  enlarged  either  by  the  radius  of  enveloping  cylinder  of  the  forearm  or  by 
that  of  the  upper  arm.  Once  all  the  secondary  obstacles  are  identified,  they  can  be 
also  sorted  by  the  angular  values  of  the  first  joint  corresponding  to  the  profile  planes 
between  starting  and  goal  positions. 

4.2.4  Hierarchical  Subdivision  of  Workspace 

For  the  PUMA  robot  (and  TRS  robots),  it  is  always  possible  to  divide  the 
robot  workspace  into  two  regions  separated  by  a hole  and  the  outer  boundary 
surfaces  of  Ww  and  Wc.  When  obstacles  are  placed  in  a robot  working  environment, 


obstacles  that  may 
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they  can  be  easily  classed  based  on  those  regions  in  which  they  are  placed.  For 
example,  one  obstacle  may  be  located  in  the  workspace  W„  while  another  obstacle 
is  placed  across  the  outer  boundary  surface  of  W, 

It  is  also  possible  to  divide  the  robot  working  space  (or,  more  compactly,  the 
RWS)  by  profile  planes  (refer  to  Figure  2.2  for  terminology)  associated  with  the 
angular  values  (4,)  of  the  first  turning  joint.  Since  profile  planes  for  the  PUMA 
robot  (as  well  as  TRS-type  robots)  are  perpendicular  to  the  X-Y  plane  of  a fixed 
coordinate  frame  (when  the  fixed  frame  is  placed  at  the  intersecting  point  of  the 
axes  of  first  two  turning  joints),  two  profile  planes  can  be  used  to  bound  each 

particular  obstacle.  The  angular  values  (4,)  of  the  first  turning  joint  corresponding 
to  supporting  profile  planes  can  be  easily  found  by  two  supporting  lines  of  the 
projection  of  enlarged  obstacles  on  the  X-Y  plane.  Therefore,  the  RWS  can  be 
subdivided  by  sorting  the  angular  values  of  the  first  turning  joint  The  subdivision 
of  the  RWS  involves  the  selective  approximation  of  obstacles  and  the  examination 
of  the  data  of  enlarged  obstacles.  The  RWS  can  be  first  divided  into  many 
subsections  by  the  supporting  profile  planes  of  the  primary  obstacles. 

Upon  completion  of  the  subdivision  of  the  RWS  by  primary  obstacles,  the 
RWS  may  be  further  subdivided  by  the  profile  planes  associated  with  secondary 
obstacles  so  that  the  RWS  bounded  by  two  surfaces  corresponding  to  two  input 
configurations  of  the  robot  is  hierarchically  divided.  In  this  process,  the  obstacles 


A plane  (linens  called  a supporting  plant  (linO  for  a convex  soUd  (figure)  if  the 


placed  within  the  workspace  of  the  elbow  point,  i.e.,  Wcl0,  are  first  considered.  If 


one  secondary  obstacle  is  placed  across  the  surface  of  Wtm,  two  enlarged  volumes 
of  this  obstacle  should  be  considered  separately  in  the  subdivision  process.  Figure 
4.3  illustrates  hierarchical  subdivision  of  the  RWS  of  a robot.  Each  interval  in  a 

42.5— Coimectiyily  Matrix 

Let  us  consider  an  example  of  the  robot  working  environment  shown  in 
Figure  4.4.  In  Figure  4.4a,  it  is  obvious  that  the  robot  arm  has  to  be  withdrawn 
from  its  starting  position  of  the  end  effector  in  order  to  execute  a collision-free  move 
towards  the  goal  position.  On  the  other  hand,  the  robot  shown  in  Figure  4.4b  can 
go  over  the  same  obstacle  without  collision.  Planning  such  a safe  move  may  be 
efficiently  carried  out  when  the  information  about  connectivity  between  obstacles  is 
provided  to  the  path  planner.  It  is  necessary  to  consider  not  only  connectivity 
between  obstacles  but  also  connectivity  of  obstacles  with  the  boundary  surface  of  W(. 
For  a practical  implementation,  two  tangential  planes  to  the  boundary  surface  of 
W„  i.e,  a sphere  Se,  which  are  parallel  to  Z = 0 plane  are  considered.  One 
tangential  plane  at  the  north  pole  is  called  a ceiling  while  the  other  at  the  south  pole 
is  called  a ground.  Thus,  obstacles  can  be  classed  into  four  categories: 

1)  TYPE  i:  obstacles  placed  on  the  ground, 

2)  TYPE  II:  obstacles  hanging  from  the  ceiling, 

3)  TYPE  III:  obstacles  attached  both  to  the  ground  and  to  the  ceiling. 


obstacles  supported  by  > 


obstacles. 
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Subdivision 


Figure  4.3  Hierarchical  Subdivision  of  the  RWS 
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Obstacles  suspended  without  any  support  (or  floating  obstacles)  are  excluded  from 

Information  about  the  connectivity  between  obstacles  can  be  stored  in  a 
matrix  form  called  a connectivity  matrix  of  obstacles  (see  Figure  4.3a).  A 
connectivity  matrix  is  a (n+2)x(n+2)  symmetrical  matrix.  Each  column  (and  each 
row)  represents  one  obstacle.  All  the  entries  of  a connectivity  matrix  are  binaries, 
i.e.,  1 (true)  and  0 (false)  which  represent  "connection*  and  "no  connection" 
respectively.  Thus,  all  the  diagonal  entries  are  l's.  Figure  4.5  is  used  to  illustrate 
one  example  of  connectivity  matrix  for  a given  RWS.  The  connectivity  matrix  can 
be  constructed  by  checking  intersections  of  expanded  obstacles  using  Eq.  (3-7).  It 
should  be  noted,  however,  that  the  connectivity  matrix  does  not  necessarily  represent 
actual  connectivity  of  obstacles  since  it  is  constructed  by  geometrical  information  of 
expanded  obstacles. 


4.2.6  Planning  Rule 
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Figure  4.5  Classification  of  Obstacles  and  Connectivity  Matrix 
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However,  since  the  connectivity  matrix  provides  information  only  about  connectivity 
between  obstacles,  applying  one  of  the  rules  to  type  iv  obstacles  demands  more 
geometrical  information  about  such  obstacles. 


consider  Figure  4.4  again.  In  Figure  4.4a,  the  robot  has  to  withdraw  its  arm  from 
a 'closed  loop'  made  by  obstacle  T,  '2',  and  ’3’  whereas  it  can  go  over  obstacle  '2' 
in  a different  environment  shown  in  Figure  4.4b.  This  shows  that  if  there  exists  any 
obstacle  which  'separates’  two  input  configurations  of  a robot,  then  a robot  should 
withdraw  its  arm  so  as  to  avoid  such  an  obstacle,  type  hi  obstacles  can  as  well  be 


RWS  of  a robot,  it  should  be  analyzed  beforehand  whether  those  obstacles  separate 
two  input  configurations  of  a robot  or  not.  When  a TYPE  IV  obstacle  is  found  not 
to  separate  two  input  configurations,  it  can  be  considered  either  TYPE  I,  II,  or  111 
obstacle  depending  on  its  type  of  support  by  connected  obstacles.  It  is  also  found 
that  if  any  type  iv  obstacle  (not  supported  by  TYPE  I and  TYPE  II  obstacles  at  the 
same  time)  are  located  in  neither  the  first  nor  the  last  planning  subsection,  it  can 
not  separate  two  input  configurations  of  a robot.  From  this  observation,  it  becomes 
clear  that  type  iv  obstacles  placed  only  in  the  first  and  the  last  subsections  require 
further  geometric  analysis  of  such  separation. 

When  a TYPE  IV  obstacle  is  found  in  the  first  and/or  the  last  planning 
subsections,  the  obstacles  supporting  TYPE  IV  obstacle  can  be  immediately  identified 
by  examining  the  connectivity  matrix.  Depending  on  the  types  of  supporting 
obstacles,  the  type  IV  obstacle  can  be  swept  either  to  the  ground  or  to  the  ceiling. 
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The  intersection  checking  of  a robot  and  the  swept  volume  of  a TYPE  IV  obstacle 
gives  an  answer  to  the  problem  whether  or  not  the  robot  is  within  a ‘dosed  loop’  of 


obstacles.  Therefore, 

we  have  the  following  subsidiary  rules  that  are  to  be  applied 

only  to  the  obstacles  1 

found  in  the  first  and  the  last  planning  subsections: 

SUBSIDIARY  Rl 

JLE  I:  go  over  TYPE  IV  obstade  if  its  TYPE  I 

supporting  obstacles  do  not  make  a 
'closed  loop'  around  the  robot  arm 

SUBSIDIARY  Rl 

ILE II:  go  underneath  TYPE  IV  obstade  if  its 

make  a 'closed  loop'  around  the  robot  arm 

SUBSIDIARY  Rl 

ILE  III:  withdraw  the  robot  arm  if  its  supporting 

It  should  be  noted  tha 

t when  intersection  of  the  robot  arm  and  the  swept  volume  of 

a type  iv  obstacle  i 

is  checked,  three  swept  volumes  corresponding  to  three 

expanded  obstacles  ha 

ive  to  be  checked  for  the  proper  part  of  the  roboL 

contains  more  than  on 

e primary  and/or  secondary  obstacle,  it  is  necessaiy  to  further 

analyze  the  connectivi 

ty  of  those  obstacles.  For  instance,  if  one  obstacle  is  put  on 

top  of  another  obstacle  on  the  ground,  this  obstacle  is  dassed  as  TYPE  I.  In  this 
case,  PLANNING  RULE  I can  be  applied  to  both  obstacles.  However,  it  is  obvious 
that  applying  the  rule  to  the  grounded  obstacle  is  not  necessary.  Such  an  obstade 


that  has  to  be  conside 

red  in  the  first  place  is  referred  to  as  a priority  obstacle.  If 

a planning  subsection 

contains  many  primary  or  secondary  obstacles  with  the  same 

types  of  support,  then 

allest  obstacle  is  selected  for  a priority  (primary  or  secondary) 

le  planning  rule  can  be  applied.  When  type  l and  type  n 

planning 


obstacle  is  seleded  for 
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a priority  obstacle.  One  planning  subsection  may  have  a priority  primaty  and/or 
secondary  obstade(s). 


A planning  blueprint  can  be  implemented  by  a doubly  linked  list  of  data 
structures  in  terms  of  computer  language.  Each  of  the  planning  subsections 
corresponds  to  a data  structure  in  a linked  list.  Each  of  the  data  structures  in  a 
linked  list  may  contain  the  following  information: 

1)  angular  values  (*,  for  the  PUMA)  at  the  boundaries 
of  a subsection, 

2)  the  number  of  primary  and  secondary  obstacles, 

3)  identifications  of  primaty  and  secondary  obstacles, 

4)  access  to  the  geometric  information  about  obstacles 
placed  in  a subsection 

5)  planning  RULES  for  the  hand,  forearm,  and  the  upper  arm, 

6)  suggested  collision-free  configurations  of  a robot 
at  the  boundaries  of  a subsection. 

When  a linked  list  representing  a planning  blueprint  is  constructed,  a straight  line 
motion  from  the  starting  position  to  the  goal  position  is  first  considered  so  as  to 
compute  the  intermediate  joint  angles  of  a robot  at  partitions  of  subsections  in  a 
blueprint.  These  joint  angles  are  stored  and  used  as  suggested  starting  and  goal 
positions  in  each  subsection. 


primaiy  obstacles  are  first  checked.  For 


the  planning  subsections  which  contain 
hand  is  first  performed. 

In  the  previous  chapter,  the  procedure  for  kinematic  analysis  of  the 

point  of  the  end  effector.  Now  let  us  consider  Figure  4.6  together  with  Figure  2.10. 
In  these  figures  it  is  clear  that,  when  one  obstacle  is  enclosed  by  the  cone  of  hand 
orientation,  the  robot  hand  can  avoid  collision  with  the  obstacle  while  its  orientation 
is  specified  in  such  a way  that  the  hand  lies  on  the  generators  of  the  enclosing  cone. 
The  enclosing  cone  may  be  determined  by  a hypothetical  hand  with  a longer  wrist 
than  the  actual  one.  However,  it  is  not  immediately  clear  how  to  determine  such 
a hypothetical  cone  of  hand  orientation  since  there  may  be  an  infinite  number  of 
feasible  choices,  in  determining  a hypothetical  cone,  two  important  factors  must 
be  considered:  1)  computational  simplicity,  and  2)  the  minimal  workspace  of  a wrist 
center  to  be  included  by  a hypothetical  cone,  i.e.,  the  maximum  feasible  volume  for 
a wrist  center.  Consideration  of  these  two  factors  suggests  an  infinitely  long  hand 
so  that  a hypothetical  cone  becomes  a hypothetical  cylinder.  For  a TRS-type  robot, 
the  axis  of  a hypothetical  cylinder  can  be  determined  by  drawing  a line  joining  the 
center  of  the  bounding  sphere  of  a primaiy  obstacle  and  the  origin  of  the  fixed 
frame.  However,  since  the  PUMA  robot  has  an  offset  on  its  shoulder,  the  axis  of 
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Figure  4.6  Hypothetical  Cylinder  of  the  Hand  of  a Robot 


hypothetical  cylir 


two  supporting 


profile  planes  of  an  obstacle,  and  the  plane  Z = 0.  Since  three  planes  intersect  at 
a point  (denoted  by  P^,  in  Figure  4.7),  the  axis  of  a hypothetical  cylinder  can  be 
drawn  by  joining  the  center  of  a bounding  sphere  of  a primary  obstacle  and  the 
intersecting  point.  Let  (S,;  S01),  (Sj;  S^),  and  (S^  0)  denote  two  supporting  profile 
planes  and  the  Z = 0 plane  in  the  form  of  Plucker  coordinates  respectively.  Then, 
the  intersecting  point  (P^,)  made  by  these  three  planes  can  be  expressed  by  the 
intersecting  point  P^:  (SI*S^«SZ;  Sm'SP&Z  + Sm'SzxS,).  The  radius  of  a 
hypothetical  cylinder  is  determined  by  that  of  a bounding  sphere.  Figure  4.6  is  used 
to  illustrate  the  determination  procedure  of  a hypothetical  cylinder  for  a primary 
obstacle.  Let  us  suppose  u is  a unit  vector  along  the  axis  of  the  hypothetical  cylinder 
drawn  from  the  intersecting  point  P,,,.  When  the  hypothetical  cylinder  of  the  hand 
is  determined,  the  obstacle  can  be  orthogonally  projected  onto  a supporting  plane 
in  the  direction  of  -u  [10].  This  particular  supporting  plane  is  referred  to  as  a 
screen  for  the  obstacle.  The  equation  of  the  screen  which  is  expressed  by  ( u;  u„ 
) in  Plucker  coordinates  can  be  determined  by  the  minimum  distance  (uj)  from  the 
origin  of  the  fixed  coordinate  frame  to  a screen. 

tio  = min  ( v,  . n ),  for  i = 1,  2, . . . , n (4-1) 

where  v,  is  a position  vector  to  a vertex  i of  an  obstacle  from  the  origin  of  the  fixed 
frame  and  n is  the  number  of  vertices  of  the  obstacle.  When  a hypothetical  cylinder 
of  the  hand  of  a robot  is  determined,  this  cylinder  may  be  modified  for  more 
geometric  accuracy  by  projecting  an  obstacle  onto  a screen.  The  projection  of  an 
obstacle  may  be  obtained  by  taking  the  convex  hull  [49,  50]  of  a set  o 


of  projected 
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Figure  4.7  Determinalion  of  a Screen 


points  of  vertices  on  a screen.  From  the  projection  of  an  obstacle  on  a screen,  a 
diameter  of  the  projected  figure,  or  a convex  hull  of  projected  points  of  the  vertices, 
can  be  also  determined  by  computing  the  maximum  distance  between  a pair  of 
vertices  of  a convex  hull.  Since  the  modified  hypothetical  cylinder  has  a smaller 
diameter  than  that  of  the  original  one,  replacing  the  original  hypothetical  cylinder 

Once  a hypothetical  cylinder  with  a screen  is  determined,  the  volume 
enclosed  by  the  cylinder  with  one  open  end  is  considered  to  be  the  boundary  of  a 
forbidden  region  for  the  robot  hand. 


1.  When  hypothetical 
cylinders  for  all  the  primaty  obstacles  are  determined,  the  orientation  of  the  hand 
can  be  maintained  such  that,  in  each  planning  subsection,  it  remains  coincident  with 
that  of  the  axis  of  hypothetical  cylinder  while  the  hand  staying  outside  the  cylinder. 
A hypothetical  cylinder  has  one  open  and  one  closed  end.  The  closed  end  is 


determined  by  the  intersection  of  an  infinitely  long  hypothetical  cylinder  and  a 
screen  which  is  perpendicular  to  the  axis  of  the  cylinder.  The  open  end  is,  therefore, 
located  at  infinity.  The  path  planning  for  a wrist  center  can  be  carried  out  outside 
the  hypothetical  cylinder  while  maintaining  the  hand  orientation  in  the  direction  of 
the  axis  of  the  hypothetical  cylinder.  The  position  of  a wrist  center,  P„  can  be 
determined  in  such  a way  that  it  moves  along  a conic  section  made  by  a hypothetical 
cylinder  and  a screen  perpendicular  to  the  axis  of  the  hypothetical  cylinder. 

When  the  position  of  the  wrist  center  P„  is  given  by  a point  on  such  a conic 
section,  the  joint  angles  (of  the  PUMA),  Sj,  and  S},  can  be  immediately 
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determined.  Since  the  hand  (of  the  PUMA)  is  to  be  aligned  with  a generator  of  a 
hypothetical  cylinder,  the  necessary  overall  mobility  of  a robot  is  five,  which 
implies  that  the  computation  of  the  first  five  joint  angles  is  necessary.  The  joint 
angles,  6,  and  ls,  on  the  wrist  mechanism  can  be  computed  as  follows  (see  Figure 
4.8):  since  the  first  three  joint  angles  are  computed  when  the  position  of  the  wrist 
center  is  specified,  we  get  the  rotation  matrix  Em  which  transforms  a vector  in  a 
fixed  coordinate  frame  (the  fust  coordinate  system)  into  a local  one  placed  at  the 
wrist  center  (3rd  coordinate  system): 

Era  = [5».  &%»§,],  (4-2) 

where  a*  = (W3J1,  -V„„  U„,),  and 

S,  = (X,„  X,;.  Zj  ).  (4-3) 

For  a wrist  mechanism  shown  in  Figure  4.8,  the  pointing  vector  S4  is  specified  by 
a generator  of  a hypothetical  cylinder.  By  transforming  this  vector  into  one  in  the 
3rd  coordinate  system,  we  get 

§6®  = Edj  §j°*.  (4-4) 

Since  this  vector  may  be  expressed  by 

- (S*.  S*  S J 

= (Xj,,  Yjj,  Zj,) 

- ( -c<Ss,  c,  S4S5).  (4-5) 

Therefore,  we  get 

I,  = tan't-S^’/S,,,01).  (4-6) 

Subsutuung  this  angle  back  into  Eq.  (4-5),  we  get 
•s  = tanW’VfeA,'3’)). 


(4-7) 


US 


Figure  4.8  Wrist  Mechanism  of  the  PUMA  and  Notation 


subsection,  suggested  starting 


and  goal  positions  associated  with  a straight  line  motion  are  replaced  with  new 

process  in  each  of  the  planning  subsections  can  be  carried  out  separately,  if 
secondary  obstacles  are  placed  in  a planning  subsection,  it  is  necessary  to  examine 

positions  are  collision-free.  If  a robot  with  these  configurations  interferes  with  any 
secondary  obstacle,  alternative  safe  configurations  of  a robot  must  be  planned. 

In  this  process,  the  permissible  angular  range  of  the  upper  arm  for  secondary 
obstacles  is  first  computed.  When  a grounded  (hanging  from  a ceiling)  obstacle  is 
detected  within  the  workspace  W,l0,  the  minimum  (maximum)  value  of  the 
permissible  angular  range  of  02  (refer  to  Figure  4.8  for  the  notation)  is  selected  since 
it  yields  the  least  movement  of  the  upper  arm  from  the  suggested  configuration.  A 
safe  orientation  of  the  forearm  can  be  computed  as  follows:  let  us  consider  Figure 
4.9  which  shows  the  cross  sectional  view  of  a working  environment  on  the  profile 
plane  of  one  boundaty  of  a subsection.  A typical  cross  sectional  view  of  a working 
environment  will  show  line  segments  for  obstacles  since  the  boundaries  of  the 
planning  subsections  correspond  to  the  supporting  profile  planes  of  obstacles. 
However,  since  it  is  also  possible  to  have  polygonal  cross  sectional  views  on  the 
boundaries  of  planning  subsections,  this  case  will  be  discussed.  In  Figure  4.9  the 
suggested  configuration  of  a robot,  represen 


nted  by  i 


slid  line  segments  for 
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the  upper  arm  and  the  forearm,  interferes  with  obstacles.  Since  the  metric 
properties  such  as  coordinates  of  the  vertices  of  obstacles  are  immediately  available 
from  a planning  blueprint,  the  image  of  the  relative  location  of  the  upper  arm  with 
respect  to  the  reference  line  segment,  which  is,  in  this  case,  edge  1 in  Figure  4.9, 
can  be  obtained  and  checked  for  intersections  at  the  boundary  of  an  image  obstacle. 

It  was  described  in  section  3.4  that  all  the  relative  displacements  of  the  upper 
arm  with  respect  to  a reference  line  segment  are  mapped  into  the  points  on  a 
hyperbola  in  a mapping  space  and  that  the  image  obstacle  that  represents  the 
geometric  condition  of  the  intersection  of  two  line  segments  is  expressed  by  the 
inequalities  (3-24).  In  Figure  4.9,  the  point  Pd  is  taken  as  a reference  point  of  a 
moving  line  segment,  i.e.,  the  upper  arm.  When  affine  coordinates  s,  and  S3  are 
attached  to  the  reference  line  segment  (edge  1)  and  the  upper  arm  respectively, 
the  orientation  of  the  upper  arm  at  should  lie  on  the  boundaty  line  s,  = 0 of 
an  image  obstacle  in  a mapping  space.  When  the  desired  orientation  of  the  upper 
arm  is  found,  the  elbow  point  P,|b2  is  taken  as  a reference  point  for  the  forearm, 
and  the  intersections  of  the  hyperbolas  and  the  boundaries  of  the  image  obstacle  can 

location  of  the  upper  arm  and/or  forearm  (hyperbola)  and  the  image  obstacle,  let 
us  consider  Eq.  (3-46), 

(s,  - x)J  - S3  + yj  = 0.  (4-8) 

Since  the  intersections,  if  any,  are  on  the  boundary  of  the  image  obstacle,  the 
coordinates  of  the  intersections  can  be  found  as  follows: 


h - ±(**  + y2)'. 

s,  - ±((a,  - x)J  + y2]*, 
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when  Sj  = -a*,  and  f s aj,  s,  = * i [(a^  - y2]*. 


If  any  of  these  coordinates  are  found  to  satisfy  the  inequalities  (3-24)  for  a priority 
(secondary)  obstacle,  the  corresponding  image  point  for  other  secondaty  obstacles 
should  be  crosschecked.  In  this  case,  the  inequalities  representing  an  image  obstacle 


(4-10) 


computed  by  exami 
acle,  edge  2 in  this  i 


ing  Eq.  (4-9)  is 
cample,  Eq.(4- 


>e  performed  to  find  a safe 


If  a safe  orientation  for  the  forean 
mapped  into  a point  within  another 
9)  is  used  again  to  find  an  altern; 
obstacles.  For  multiple  obstacles,  t 
configuration  for  the  forearm. 

It  should  be  noted  that  planning  for  a collision-free  configuration  at  each 
boundary  of  the  planning  subsections  makes  it  possible  to  perform  the  entire  path 
planning  process  on  an  independent  subsection-basis.  It  is  also  noted  that  taking  a 
subsection  as  a unit  of  path  planning  provides  a symmetry  for  the  entire  planning 
process.  Once  the  collision-free  configurations  are  determined,  the  direction  of 
planning  becomes  unimportant. 

t.  When  the  collision- 


free  configurations  at  starting  and  goal  positions  in  each  of  the  planning  subsections 
are  determined,  the  image  points  in  a mapping  space  corresponding  to  these 
configurations  can  be  computed  from  Eq.  (3-39).  By  examining  the  geometric 
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information  about  obstacles  through  a blueprint,  a priority  secondary  obstacle  is 
chosen  for  planning  in  each  of  the  planning  subsections  while  other  secondary 
obstacles  are  checked  for  interference  with  the  forearm.  By  utilizing  the  information 

the  type  of  the  obstacle,  the  top  (bottom)  face  of  a type  i (type  n)  priority 
secondary  obstacle  is  selected  for  the  reference  plane  in  planning.  For  a type  i 
priority  obstacle,  the  wrist  point  is  taken  as  a reference  point  of  a moving  line 

point  for  a TYPE  n obstacle.  The  same  procedure  as  that  used  for  a safe 
configuration  of  the  forearm  at  a subsection  boundary  is  applied  to  the  forearm 
planning  process  only  when  a secondary  obstacle  is  detected  within  the  workspace 

In  section  4.2.1,  it  was  described  that  this  implementation  of  the  theory  for 
articulation  deals  with  obstacles  with  fiat  top  or  bottom  faces.  However,  in  theory, 
when  a priority  secondary  obstacle  is  modeled  by  a space  segment  and  oriented  in 
such  a way  that  it  does  not  have  a fiat  top  face,  some  slope  faces  of  such  an 
obstacle  should  be  selected  for  reference  plane  segments.  Although  the  identifying 
process  of  those  slope  faces  may  be  done  by  developing  some  intelligent  geometrical 
algorithms,  one  simple  technique  may  be  to  use  a hypothetical  cylinder  for  an 
obstacle  and  examine  the  visibility  of  the  faces  of  an  obstacle  from  the  intersecting 
point  P^,  by  taking  a dot  product  of  a normal  vector  coming  out  of  the  faces  and  a 


' along  an  axis  of  a hypothetical  cylinder.  Those  faces  of  an  obstacle  I 
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are  noi  visible  from  Ihe  point  P,,,  can  be  selected  for  reference  plane  segments  in 
planning. 

For  the  collision-free  configurations  at  starting  and  goal  positions  in  a 
planning  subsection,  a straight  line  motion  or  a planned  path  of  the  hand  is  first 
checked  for  the  feasibility  of  a collision-free  path.  If  the  straight  line  motion  or 
planned  path  for  the  hand  turns  out  to  be  infeasible,  a planning  for  the  regional 
structure  has  to  be  carried  out.  Having  taken  a top  or  a bottom  face  of  a priority 
secondary  obstacle  as  a reference  plane  depending  on  the  type  of  obstacle,  an  image 
obstacle  represented  by  inequalities  (3-42)  or  (3-37)  in  a mapping  space  can  be 
determined  for  the  forearm  of  a robot.  In  a mapping  space,  a line  segment  joining 
those  two  image  points  can  be  constructed  and  checked  for  interference  with  an 
image  obstacle.  When  no  interference  occurs,  it  is  possible  to  obtain  collision-free 
configurations  of  the  forearm  by  taking  the  inverse  mapping  in  the  planning 
subsecdon  while  utilizing  the  informauon  of  geometrical  capability  of  the  forearm 
for  a specified  position  and  orientation  of  the  hand  of  a robot.  Figure  4.10  is  used 
to  illustrate  the  concept  of  the  application  of  the  inverse  mapping. 

When  a line  segment  joining  two  image  points  interferes  with  an  image 

compute  the  intersecting  points  of  an  image  obstacle  and  the  line  segment.  For  the 
simplest  planning  in  the  mapping  space,  an  intermediate  point(s)  on  an  edge(s)  of 
the  image  obstacle,  i.e.,  the  corner  point(s)  of  the  image  obstacle,  is  used  to 
construct  a series  of  line  segments  connecting  the  intersecting  points.  This  series  of 


n.  Figure  4.11 


Figure  4.10  Application  of  the  Inverse  Mapping  for  Planning  to  coQ&on- 
free  path  for  the  forearm  in  planning. 
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direction  which  leads  the  series  of  line  segments  from  the  initial  image  point  to  the 

on  a reference  plane  segment,  the  sense  of  the  direction  from  an  initial  image  point 
to  a final  one  via  intermediate  image  points  determines  the  type  of  robot  arm  move. 
In  other  words,  since  a planning  rule  is  selected  in  advance  when  the  blueprint  is 
constructed  and  the  connectivity  is  checked,  the  sense  of  direction  is  consequently 
determined.  For  example,  if  the  intermediate  corner  points  on  a suggested  image 
path  in  a mapping  space  are  determined  as  shown  in  Figure  4.1 1,  the  corresponding 
forearm  move  for  the  suggested  image  path  is  to  go  over  (PLANNING  RULE  t)  a 
priority  secondary  obstacle.  On  the  other  hand,  the  suggested  image  path  via  the 
origin  of  the  s,-s2  coordinate  system  embedded  on  a reference  plane  segment  will 
result  in  a withdraw  motion  of  the  forearm.  For  an  applied  rule  to  a priority 
secondary  obstacle,  the  intermediate  comer  points  on  a suggested  image  path  may 
be  determined  by  using  the  following  determinant: 

*2  y2  1 J (4-11) 

which  represents  the  area  of  a plane  segment  made  by  three  points.  The  sign  of  this 
determinant  depends  on  the  order  in  which  the  points  are  taken.  If  the  points  are 
taken  counterclockwise,  it  gives  a positive  value.  In  Figure  4.11,  the  intermediate 
corner  points  of  a suggested  image  path  which  give  a positive  value  of  the 
determinant  have  been  selected  for  given  initial  and  final  image  points.  The 
determination  of  the  intermediate  comer  points  at  this  stage  gives  the  s,  and  Sj 
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Figure  4.11  A Suggested  Path  in  a Mapping  Space 
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coordinates  of  those  points.  The  s,  coordinates  of  the  intersecting  points  of  a line 
segment  joining  initial  and  final  image  points,  and  an  image  obstacle,  can  be  used 
for  those  of  intermediate  comer  points  of  a suggested  image  path.  This  implies  the 
translation  of  the  intersecting  points  to  the  edges  on  83  ( = constant)  planes  (see 
Figure  4.11).  This  completes  the  procedure  of  obtaining  a suggested  image  path  in 
a mapping  space. 

For  a relative  displacement  of  a line  segment  with  respect  to  a reference 
plane  segment,  an  image  obstacle  was  expressed  by  inequalities  (3-42),  which 
represented  the  geometric  condition  of  interference  of  two  objects.  When  a moving 

infeasible  region  of  image  points  other  than  the  image  obstacle,  due  to  the 
geometric  constraint  of  the  forearm  of  a robot.  Since  there  may  be  an  unreachable 
workspace  (sometimes  called  a shadow!  for  the  wrist  center  because  of  an  obstacle 
placed  in  W„  the  unreachable  workspace  for  the  wrist  center  may  be  mapped  into 

modeled  by  a box  placed  in  the  workspace  of  the  wrist  center  W,  When  the  top 
face  of  this  obstacle  is  taken  as  a reference  plane  segment2  for  planning,  while  the 
forearm  is  taken  as  a moving  line  segment,  the  image  of  the  unreachable  workspace 

shows  the  boundary  curve  of  the  unreachable  workspace  of  the  wrist  center  on  a 


If  the  reference  plane  is  considered  to  be  a bottom  face  of  a type  11  obstacle,  the 
shadow  should  be  the  mirror  image  of  that  of  the  TYPE  I obstacle  about  the 


Figure  4.12  Mapping  of  Ihe  Unreachable  Workspace  of  the  Wrist  Center 
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profile  plane.  It  was  illustrated  in  section  2.5  that  the  regional  structure,  in  this 
case,  can  be  modeled  as  an  inverted  slider-crank  mechanism  on  a profile  plane  at 
the  current  0,  angle.  Then,  the  boundary  curve  of  the  unreachable  workspace  for 
the  wrist  center  is  represented  by  a coupler  curve  of  the  wrist  point,  which  is  a 
sextic  (51].  Now  let  us  consider  Figure  4.12b.  This  figure  illustrates  the  relationship 

wrist  center  to  the  pivot  T.  A close  investigation  of  Figure  3.8  reveals  the  fact  that 
the  absolute  value  of  the  coordinate  s,  of  the  image  point  is  the  same  as  the  length 
h.  By  computing  the  maximum  length  h for  given  position  of  the  pivot,  the 
unreachable  workspace  of  the  wrist  center  can  be  mapped  into  the  shaded  region  in 
Figure  4.12c,  which  represents  the  infeasible  region  of  the  image  points.  It  should 
be  noted  here  that  the  image  points  on  the  boundary  curve  of  the  shaded  region  in 
a mapping  space  must  be  included  in  the  infeasible  region  since  those  points  are  not 

of  the  mapping  of  the  unreachable  workspace  of  the  wrist  center  into  a certain 
region  in  a mapping  space,  the  inverse  mapping  of  the  path  consisting  of  a series 

collision-free  path  of  the  forearm.  In  this  process,  the  path  must  be  checked  to 
determine  if  it  stays  outside  the  infeasible  region  representing  the  unreachable 
workspace  of  the  wrist  center.  When  any  point  on  the  suggested  line  path  is  found 
to  be  in  the  infeasible  region,  that  point  is  replaced  by  the  boundary  point  of  the 
ble  region  for  an  image  point  for  the  forearm  move.  However,  because  the 


boundary  points  are  not  obtainable,  a small  number  (Iff5  was  used  in  the  computer 
implementation)  can  be  used  to  avoid  the  singularity  problem  in  computation.  Such 
a replacement  of  an  image  point  corresponds  to  a small  relative  rotational  motion 
of  the  forearm  about  the  pivot  T in  E3. 

Let  us  suppose  now  that  an  image  point  P*  (■  (s„  Sp  S3))  in  a feasible  region 
is  specified.  Then,  the  next  step  is  to  find  a safe  position  and  orientation  of  the 
forearm.  From  Eq.  (3-49a),  we  had 

X - S,  + lj,Sj  + Ij,Sj 

y = ajfl,  + raj*  (3-49a) 

In  order  to  compute  the  relative  position  PE  («  [x,  y,  z]1)  of  a reference  point  of  the 
forearm  from  this  equation,  it  is  necessaiy,  first  of  all,  to  obtain  a feasible  relative 
orientation  [l3I.  m3l,  n„]  of  the  forearm.  It  was  shown  that  an  absolute  value  of  a 
given  s3  coordinate  of  an  image  point  is  the  same  as  the  length  h of  a coupler  link 
of  an  inverted  slider-crank  mechanism  extruded  from  the  pivot  T.  Since  the 

information  of  the  expanded  obstacle,  it  is  possible  to  get  the  orientation  of  the 
forearm  for  a given  value  of  h.  Let  PT  (.  (x*  0,  Zf)"3)  and  P[lb  (-  (x^  0,  z*,)">) 
denote  the  position  vectors  from  the  origin  of  the  first  local  coordinate  system  (refer 
to  Figure  4.8  for  notation)  to  the  pivot  and  the  elbow  points,  respectively.  The 
elbow  points  are  determined  by  the  intersections  of  two  circles,  i.e.,  a constraint 
circle  Cj  and  a circle  with  its  center  at  point  T.  Using  Eq.  (2-52)  and  Eq.  (2-53), 
the  coordinates  of  the  elbow  points  are  then  given  by: 

*.»  “ CpO,«,  - S/A  ib 
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zeB>  ~ Spae)b  + Off* 

C p = cos(p)  and  = 


l[(2a23dr);  - (aj  - (a„  - h)1  + dj}2)* 
2dr 


= *r  + *p 

and  d = tan'tif/Xr). 

The  positive  value  (elbow-up  configuration)  of  0efb  is  chosen  for  a TYPE  I obstacle 

maintain  consistency  of  the  notation  convention  from  chapter  3,  let  gj1*  be  a unit 
vector  along  the  forearm,  while  remembering  that  g,  (*  [1,„  m,„  n,,]1)  and  g,  (■  [la, 
m^,  nj')  are  the  unit  vectors  representing  a reference  plane  segment,  such  that 
g?’  * I'si.  t>3t.  %J' 

PT  “ Peu, 

i£t-pj  (444> 

wrist  center  Pw  for  a type  i obstacle,  can  now  be  determined  by  Eq.  (3-49a). 
Accordingly,  the  position  of  the  reference  point  of  the  forearm  in  the  fixed 
coordinate  system  is  computed  from  Eq.  (3-13): 

P = EPe  + PQ 


(4-15) 
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where  the  column  vectors  of  E are  «„  g, xgj.  and  (gixgjxjt,  respectively  and  P0  is 
the  position  vector  from  the  origin  of  the  fixed  frame  to  the  origin  of  the  moving 
coordinate  system  embedded  in  a reference  plane  segment.  It  is  important,  however, 
to  consider  the  restriction  on  the  position  of  the  reference  point  of  the  forearm. 
Since  the  planning  for  the  hand  was  carried  out  beforehand,  the  path  generated  for 
the  hand  is  used  as  the  minimum  (maximum)  height  of  the  wrist  center  for  a TYPE 
i (TYPE  ll)  secondaty  obstacle.  Thus,  if  the  z coordinate  of  the  computed  position 
PE  does  not  satisfy  the  minimum  (or  maximum)  height,  the  z coordinate  of  the  wrist 
center  is  replaced  by  the  minimum  (maximum)  height.  The  joint  angles  for  the 
regional  structure  are  then  computed  to  replace  the  planned  values. 

any  interference  of  the  forearm  with  those  obstacles  is  detected,  the  planned 
position  and  orientation  of  the  forearm  need  to  be  modified.  It  is  clear  that,  when 
any  interference  of  a planned  position  and  orientation  of  the  forearm  with  other 
type  I or  type  ll  secondaty  obstacles  is  detected,  there  could  be  only  two  possible 

in  this  case,  it  is  necessary  to  regenerate  the  collision-free  path  for  the  hand  by 
taking  the  interfering  secondaty  obstacle  into  consideration.  The  same  procedure 
as  that  used  for  planning  for  the  hand  is  used  again  and  the  planning  for  the  forearm 
has  to  be  modified  in  a current  planning  subsection.  Then,  the  planning  for  the 
forearm  is  carried  out  for  a modified  collision-free  path  of  the  regional  structure. 
It  should  be  noted  here  that  this  modifying  process  is  not  repetitive.  The  second 
case  of  interference  is  possible  when  the  forearm  interferes  with  a different  type  of 


obstacle.  Here,  the  intersecting  point  which  can  be  "seen"  from  the  reference  point 
of  the  forearm  is  considered  in  order  to  find  a safe  orientation  of  the  forearm. 
Depending  on  the  type  of  obstacle,  the  intersecting  point  can  be  vertically  displaced 
onto  a reference  plane  and  joined  to  the  wrist  center  by  a line  segment.  The  newly 
generated  line  segment  is  then  used  to  compute  a new  orientation  of  the  forearm 
and  thus,  a new  position  of  the  wrist  center. 


collision-free  path  for  the  PUMA  robot  was  implemented  on  the  Silicon  Graphics 
IRIS  4D/70  computer,  a UNIX-based  graphics  workstation.  The  program  was 
written  in  C language  and  a built-in  graphics  library  on  the  system  was  used  for  basic 
graphics  functions  such  as  line  draw,  polygon  fill,  and  so  on.  The  solid  models  for 
the  robot  working  environment  and  the  PUMA  robot  were  built  by  utilizing  a Z- 
buffer  algorithm  provided  by  the  system. 

The  underlying  assumption  of  obstacle  avoidance  was  that  all  the  geometric 
information  about  the  obstacle  is  provided.  In  the  implementation,  the  input 
geometric  data  of  obstacles  were  the  dimensions  of  sides,  the  position  and 
orientation  of  a local  coordinate  system  attached  to  the  obstacle,  and  its  shape. 
The  geometric  data  of  obstacles  were  stored  in  the  data  structures  which  also 
contained  the  preprocessed  geometric  data  including  the  information  about  the 
expanded  obstacles,  enclosing  hypothetical  cylinder,  the  equation  of  the  screen, 
and  all  the  numerical  properties  related  to  the  affine  transformation  for  static 
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Eq.  (3-6).  When  the  primary  and  secondary  obstacles  were  identified,  a connectivity 
matrix  was  constructed  by  analyzing  the  geometric  data  of  the  expanded  obstacles. 
The  planning  blueprint  was  implemented  by  use  of  a doubly  linked  list. 

PUMA  robot  was  performed  for  a primary  path  in  order  to  generate  the  suggested 
starting  and  goal  positions  and  orientations  at  each  boundary  of  planning  subsections. 
In  order  to  locate  an  exact  planning  subsection  (corresponding  to  a data  structure) 
for  any  particular  obstacle  in  the  RWS,  the  angular  values  corresponding  to  the 
supporting  profile  planes  of  the  primaty  and  the  secondary  obstacle  were  analyzed. 
Having  completely  constructed  the  blueprint,  the  possibility  of  separating  two 

the  obstacles  located  in  each  planning  subsection  were  selected  for  the  hand,  the 

planning  process  was  performed,  the  data  of  a collision-free  path  were  also  stored 
in  corresponding  data  lists.  Figure  4.13  illustrates  the  mechanism  of  the  blueprint 
and  the  data  structures  for  the  geometric  data  of  obstacles  used  in  this 
implementation. 

The  application  to  obstacle  avoidance  demonstrated  in  the  Silicon  Graphics 
Computer  showed  that  collision-free  path  planning  for  a given  robot’s  working 
environments  can  be  carried  out  within  a several  seconds,  including  the  preparation 
of  graphics  objects.  The  following  figures  show  consecutive  configurations  of  the 
PUMA  along  planned  collision-free  paths  in  different  working  environments.  In 
Figure  4.14,  two  primary  and  one  secondary  obstacles  (all  TYPE  I 


obstacles) 


133 


information 

of 

obstacles 


Figure  4.13  The  Mechanism  of  the  Blueprint  in  the  Computer  Implementation 


placed  in  the  RWS  of  the  PUMA  while  Figure  4.15  shows  type  I and  it  obstacles 
in  the  RWS.  In  Figure  4.16,  a type  iv  obstacle  supported  by  two  type  I obstacles 
separates  two  input  configurations  of  the  PUMA  and  the  figure  shows  a collision- 
free  path  which  leads  the  robot  arm  out  of  a possible  entrapment  to  avoid  collision 
with  obstacles.  Figure  4.17  demonstrates  the  capability  of  the  path  planner  to  deal 


with  a rather  ■ 


Figure  4.15 


Figure  4.16 
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Figure  4.17 


CHAPTER  5 

CONCLUSIONS  AND  RECOMMENDATIONS 


The  theory  for  robot  articulation  for  spatial  robots  with  special  geometries 
such  as  the  PUMA  and  TRS  robots  has  been  developed  and  applied  to  a collision- 
free  path  planning.  The  geometrical  capabilities  of  those  robots  have  been  analyzed 
and  utilized  in  collision-free  path  planning.  The  geometry  of  interference  is  unique 
in  the  aspects  of  its  mathematical  development  and  potential  applicability  in  robot 
motion  planning.  The  blueprint  algorithm  was  introduced  as  the  computer 
implementation  technique  of  machine  intelligence  via  robot  geometry  for  collision- 
free  path  planning. 

Although  the  research  results  demonstrate  that  the  theoty  for  robot 
articulation  can  be  best  served  for  an  automatic  robot  motion  planning  as  a powerful 
tool,  it  does  not  solve,  by  any  means,  the  general  problem  of  obstacle  avoidance 
for  a spatial  robot.  However,  it  is  hoped  that  this  geometrical  approach  to  the 
obstacle  avoidance  problem  can  help  facilitate  many  practical  applications  in  various 
areas  of  motion  planning. 

This  research  may  be  extended  in  many  different  ways  as  follows: 

1)  consideration  of  the  joint  limits  in  path  planning, 

2)  path  planning  for  two  cooperating  robots, 

3)  path  planning  for  two  cooperating  robots  amongst  obstacles, 
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vjih  a prismatic  joir 


4)  application  of  the  theory  to  a robot ' 

(for  example,  the  Stanford  Arm). 

One  immediate  expansion  of  this  work  is  to  take  the  joint  limits  of  the  robots  into 
consideration.  The  theory  of  robot  articulation  can  be  applied  to  the  safety  problem 
for  two  cooperating  robots  in  an  overlapped  region  of  their  workspaces.  Primary 
cylinders  can  be  used  to  check  the  potential  interference  of  the  hands  of  two  robots 
and  the  relative  displacement  of  one  hand  with  respect  to  the  other  can  be  first 
considered  while  maintaining  a reference  point  of  one  hand  (i.e.,  a reference  line 
segment)  along  a primary  path.  The  blueprint  algorithm  could  be  applied  to  the 
problem  of  two  cooperating  robots  operated  amongst  stationary  obstacles.  The 
selective  approximation  technique  will  become  more  useful  when  two  distinct  robots 
are  to  be  dealt  with. 
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